inholland/ros2-homework
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat: Add attempt to implementing Z direction

Browse Source
This commit is contained in:
Wessel T
2025-11-27 12:11:52 +01:00
parent 6cb924e8e9
commit bb4d11c13f
1 changed files with 43 additions and 15 deletions
Download Patch File Download Diff File Expand all files Collapse all files

58
src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp
View File

@@ -9,10 +9,12 @@
class JointPublisherNode: public rclcpp::Node {
public:
JointPublisherNode(): Node("ik_node") {
L0_ = this->declare_parameter<double>("L0", 0.1105); // Base height to L2 joint
L1_ = this->declare_parameter<double>("L1", 0.18);
L2_ = this->declare_parameter<double>("L2", 0.0435);
target_x_ = this->declare_parameter<double>("target_x", 0.0);
target_y_ = this->declare_parameter<double>("target_y", 0.0);
target_z_ = this->declare_parameter<double>("target_z", 0.0);
case_number_ = this->declare_parameter<int>("case_number", 0);
calculateInverseKinematics(case_number_);
@@ -51,6 +53,8 @@ private:
double target_x_;
double target_y_;
double target_z_;
double L0_; // Base height
double L1_;
double L2_;
int case_number_;
@@ -79,6 +83,14 @@ private:
target_y_ = param.as_double();
recalculate = true;
RCLCPP_INFO(this->get_logger(), "Updated target_y to %f", target_y_);
} else if (param.get_name() == "target_z") {
target_z_ = param.as_double();
recalculate = true;
RCLCPP_INFO(this->get_logger(), "Updated target_z to %f", target_z_);
} else if (param.get_name() == "L0") {
L0_ = param.as_double();
recalculate = true;
RCLCPP_INFO(this->get_logger(), "Updated L0 to %f", L0_);
} else if (param.get_name() == "L1") {
L1_ = param.as_double();
recalculate = true;
@@ -114,50 +126,62 @@ private:
}
void calculateInverseKinematics(int case_number) {
double x, y;
double x, y, z;
// Use target_x_ and target_y_ if explicitly set via parameters
// Use target_x_, target_y_, target_z_ if explicitly set via parameters
// Otherwise use predefined case positions
bool use_target_params = (target_x_ != 0.0 || target_y_ != 0.0);
bool use_target_params = (target_x_ != 0.0 || target_y_ != 0.0 || target_z_ != 0.0);
if (use_target_params) {
x = target_x_;
y = target_y_;
z = target_z_;
RCLCPP_INFO(this->get_logger(), "Using explicit target position");
} else {
switch(case_number) {
case 1:
x = 0.20;
y = 0.10;
z = 0.05;
break;
case 2:
x = 0.15;
y = 0.15;
z = 0.08;
break;
case 3:
x = 0.18;
y = 0.05;
z = 0.10;
break;
case 4:
x = 0.12;
y = 0.12;
z = 0.06;
break;
default:
x = 0.15;
y = 0.10;
z = 0.05;
break;
}
RCLCPP_INFO(this->get_logger(), "Using case %d position", case_number);
}
RCLCPP_INFO(this->get_logger(), "Calculating IK for target position: x=%f, y=%f", x, y);
RCLCPP_INFO(this->get_logger(), "Calculating IK for target position: x=%f, y=%f, z=%f", x, y, z);
// theta_2 (law of cosines)
double cos_theta_2 = (x*x + y*y - L1_*L1_ - L2_*L2_) / (2 * L1_ * L2_);
// Adjust z for base height (z is measured from world, but we need z from L2 joint)
double z_from_base = z - L0_;
// Calculate the horizontal reach needed (r) and account for vertical component
double r = std::sqrt(x*x + y*y); // horizontal distance from base
// theta_2 (law of cosines) - using r and z_from_base for 2D arm planning
double reach_distance = std::sqrt(r*r + z_from_base*z_from_base);
double cos_theta_2 = (reach_distance*reach_distance - L1_*L1_ - L2_*L2_) / (2 * L1_ * L2_);
// Check if solution is valid
if (cos_theta_2 < -1.0 || cos_theta_2 > 1.0) {
RCLCPP_WARN(this->get_logger(), "target position is out of reach, using default angles");
RCLCPP_WARN(this->get_logger(), "target position (r=%f, z=%f) is out of reach, using default angles", r, z_from_base);
theta_1_ = 0.0;
theta_2_ = 0.0;
theta_3_ = 0.0;
@@ -173,19 +197,23 @@ private:
double k1 = L1_ + L2_ * cos_theta_2;
double k2 = L2_ * sin_theta_2;
// Calculate the angle to the target in the r-z plane
double alpha = std::atan2(z_from_base, r);
double gamma = std::atan2(k2, k1);
double theta_1 = std::atan2(y, x) - gamma;
double theta_1 = alpha - gamma;
// Base rotation to point towards (x, y)
double theta_0 = std::atan2(y, x);
// Convert to robot-specific joint angles
double theta_0 = 0.0; // Base rotation (theta_0)
theta_1_ = theta_0; // L1 joint
theta_2_ = theta_1 - M_PI / 2; // L2 joint
theta_3_ = theta_2; // L3 joint
theta_4_ = -theta_1 - theta_2 - M_PI / 2; // L4 joint
theta_5_ = theta_0; // L5 joint
// theta_1_ = theta_0; // L1 joint - base rotation
theta_2_ = theta_1 - M_PI / 2; // L2 joint - shoulder
theta_3_ = theta_2; // L3 joint - elbow
theta_4_ = -theta_1 - theta_2 - M_PI / 2; // L4 joint - wrist to point down
theta_5_ = theta_0; // L5 joint - wrist rotation
theta_6_ = 0.0091; // L6 joint (fixed)
RCLCPP_INFO(this->get_logger(), "Calculated angles - theta_1: %f, theta_2: %f", theta_1, theta_2);
RCLCPP_INFO(this->get_logger(), "Calculated angles - base: %f, theta_1: %f, theta_2: %f", theta_0, theta_1, theta_2);
}
};