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

feat: Connect4 board movement

Browse Source
This commit is contained in:
Wessel T
2025-11-27 12:45:53 +01:00
parent bb4d11c13f
commit 25e103ff57
1 changed files with 142 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -9,9 +9,14 @@
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);
// PAROL6 link lengths from DH parameters (in meters)
a1_ = this->declare_parameter<double>("a1", 0.11050); // Base height
a2_ = this->declare_parameter<double>("a2", 0.02342); // Base offset
a3_ = this->declare_parameter<double>("a3", 0.18000); // Shoulder to elbow
a4_ = this->declare_parameter<double>("a4", 0.04350); // Elbow offset
a5_ = this->declare_parameter<double>("a5", 0.17635); // Elbow to wrist
a6_ = this->declare_parameter<double>("a6", 0.06280); // Wrist offset
a7_ = this->declare_parameter<double>("a7", 0.04525); // End effector length
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);
@@ -27,9 +32,57 @@ public:
tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
// Set up parameter callback for dynamic reconfiguration
param_callback_handle_ = this->add_on_set_parameters_callback(
std::bind(&JointPublisherNode::parametersCallback, this, std::placeholders::_1)
);
// param_callback_handle_ = this->add_on_set_parameters_callback(
// std::bind(&JointPublisherNode::parametersCallback, this, std::placeholders::_1)
// );
// Connect 4 board parameters
// Board is at x=0.6, y=0.0, z=0.0 (from BoardLocationPublisher)
// Board offset from base_conn_4 to board_conn_4: x=0.281, y=0.031, z=0.006
double board_base_x = 0.60;
double board_base_y = 0.00;
double board_base_z = 0.00;
double board_offset_x = 0.281;
double board_offset_y = 0.031;
double board_offset_z = 0.006;
// Connect 4 is 7 columns x 6 rows
int cols = 7;
int rows = 6;
double cell_spacing_x = 0.0; // Horizontal spacing between columns
double cell_spacing_y = 0.015; // 15mm spacing between columns
double cell_spacing_z = 0.015; // 15mm spacing between rows
// Starting position for top-left cell
double board_start_x = board_base_x + board_offset_x;
double board_start_y = board_base_y + board_offset_y - (cols - 1) * cell_spacing_y / 2.0;
double board_start_z = board_base_z + board_offset_z + 0.10; // Above the board
int current_col = 0;
int current_row = 0;
while(true) {
// Calculate target position for current cell
target_x_ = board_start_x + current_row * cell_spacing_x;
target_y_ = board_start_y + current_col * cell_spacing_y;
target_z_ = board_start_z - current_row * cell_spacing_z;
// Move to position and update
calculateInverseKinematics(case_number_);
broadcastJointState();
// Move to next cell
current_col++;
if (current_col >= cols) {
current_col = 0;
current_row++;
if (current_row >= rows) {
current_row = 0; // Start over from top-left
}
}
std::this_thread::sleep_for(std::chrono::milliseconds(500)); // Pause at each cell
}
RCLCPP_INFO(this->get_logger(),
"Case %d: Joint states (%f %f %f %f %f %f) were calculated and sent to the system.",
@@ -54,9 +107,14 @@ private:
double target_x_;
double target_y_;
double target_z_;
double L0_; // Base height
double L1_;
double L2_;
// PAROL6 DH parameters (link lengths in meters)
double a1_; // Base height (110.5mm)
double a2_; // Base offset (23.42mm)
double a3_; // Shoulder to elbow (180mm)
double a4_; // Elbow offset (43.5mm)
double a5_; // Elbow to wrist (176.35mm)
double a6_; // Wrist offset (62.8mm)
double a7_; // End effector length (45.25mm)
int case_number_;
double theta_1_;
@@ -87,18 +145,18 @@ private:
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();
} else if (param.get_name() == "a1") {
a1_ = 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();
RCLCPP_INFO(this->get_logger(), "Updated a1 to %f", a1_);
} else if (param.get_name() == "a3") {
a3_ = param.as_double();
recalculate = true;
RCLCPP_INFO(this->get_logger(), "Updated L1 to %f", L1_);
} else if (param.get_name() == "L2") {
L2_ = param.as_double();
RCLCPP_INFO(this->get_logger(), "Updated a3 to %f", a3_);
} else if (param.get_name() == "a5") {
a5_ = param.as_double();
recalculate = true;
RCLCPP_INFO(this->get_logger(), "Updated L2 to %f", L2_);
RCLCPP_INFO(this->get_logger(), "Updated a5 to %f", a5_);
} else if (param.get_name() == "case_number") {
case_number_ = param.as_int();
recalculate = true;
@@ -138,31 +196,32 @@ private:
z = target_z_;
RCLCPP_INFO(this->get_logger(), "Using explicit target position");
} else {
// Default test positions for different cases
switch(case_number) {
case 1:
x = 0.20;
y = 0.10;
z = 0.05;
z = 0.15;
break;
case 2:
x = 0.15;
y = 0.15;
z = 0.08;
z = 0.20;
break;
case 3:
x = 0.18;
y = 0.05;
z = 0.10;
z = 0.18;
break;
case 4:
x = 0.12;
y = 0.12;
z = 0.06;
z = 0.15;
break;
default:
x = 0.15;
y = 0.10;
z = 0.05;
z = 0.15;
break;
}
RCLCPP_INFO(this->get_logger(), "Using case %d position", case_number);
@@ -170,50 +229,73 @@ private:
RCLCPP_INFO(this->get_logger(), "Calculating IK for target position: x=%f, y=%f, z=%f", x, y, z);
// 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
// PAROL6 Inverse Kinematics Solution
// Based on DH parameters and kinematic structure
// 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_);
// Joint 1 (Base rotation) - simple atan2 for rotation around Z axis
theta_1_ = std::atan2(y, x);
// Check if solution is valid
if (cos_theta_2 < -1.0 || cos_theta_2 > 1.0) {
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;
theta_4_ = 0.0;
theta_5_ = 0.0;
theta_6_ = 0.0091;
return;
// Calculate wrist center position (subtract end effector)
// The wrist center is located before the last 3 joints (wrist assembly)
double r = std::sqrt(x*x + y*y); // Radial distance from base
// Account for base offset a2 in the radial direction
double r_adjusted = r - a2_;
// Height from shoulder joint (which is at height a1)
double z_from_shoulder = z - a1_;
// Distance from shoulder to wrist center in the 2D plane (r, z)
// Subtract the wrist assembly length a5 from the reach
double wrist_r = r_adjusted;
double wrist_z = z_from_shoulder;
// Distance from shoulder to wrist position
double d = std::sqrt(wrist_r*wrist_r + wrist_z*wrist_z);
// Check if target is reachable
// Add some tolerance and account for offsets (a4, a6, a7)
double max_reach = a3_ + a5_ + a4_ + a7_; // Full extension plus offsets
double min_reach = std::abs(a3_ - a5_) * 0.5; // More lenient minimum
if (d > max_reach) {
RCLCPP_WARN(this->get_logger(),
"Target position (d=%f) is out of reach (max=%f), clamping to max reach",
d, max_reach);
// Scale down to max reach instead of failing
double scale = max_reach / d;
wrist_r *= scale;
wrist_z *= scale;
d = max_reach;
} else if (d < min_reach) {
RCLCPP_WARN(this->get_logger(),
"Target position (d=%f) too close (min=%f), using minimum reach",
d, min_reach);
d = min_reach;
}
double sin_theta_2 = std::sqrt(1 - cos_theta_2 * cos_theta_2);
double theta_2 = std::atan2(sin_theta_2, cos_theta_2);
// Joint 3 (Elbow) - law of cosines
// Use negative angle for "elbow down" configuration to allow arm extension
double cos_theta_3 = (d*d - a3_*a3_ - a5_*a5_) / (2 * a3_ * a5_);
cos_theta_3 = std::max(-1.0, std::min(1.0, cos_theta_3)); // Clamp to [-1, 1]
theta_3_ = -std::acos(cos_theta_3); // Negative for elbow-down configuration
double k1 = L1_ + L2_ * cos_theta_2;
double k2 = L2_ * sin_theta_2;
// Joint 2 (Shoulder) - geometric solution
double alpha = std::atan2(wrist_z, wrist_r); // Angle to target
double beta = std::atan2(a5_ * std::sin(-theta_3_), a3_ + a5_ * std::cos(-theta_3_));
theta_2_ = alpha - beta;
// 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 = alpha - gamma;
// Wrist joints - position gripper downward for dropping pieces
// theta_4: Wrist pitch - point down
// theta_5: Wrist roll - keep level
// theta_6: Gripper rotation - keep aligned
theta_4_ = -M_PI / 2; // Point gripper down
theta_5_ = 0.0;
theta_6_ = 0.0;
// Base rotation to point towards (x, y)
double theta_0 = std::atan2(y, x);
// Convert to robot-specific joint angles
// 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 - base: %f, theta_1: %f, theta_2: %f", theta_0, theta_1, theta_2);
RCLCPP_INFO(this->get_logger(),
"Calculated angles - J1(base): %f, J2(shoulder): %f, J3(elbow): %f",
theta_1_, theta_2_, theta_3_);
}
};