From c951c9e537a75c85e6b60cb0f1edcdcfea37e910 Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Sat, 13 Dec 2025 21:43:43 +0100 Subject: [PATCH] feat: Working connect4 IK --- .../CMakeLists.txt | 3 +- .../launch/inverse_kinematic.launch.xml | 23 +- .../package.xml | 1 - .../src/InverseKinematicNode.cpp | 330 +++++------------- 4 files changed, 93 insertions(+), 264 deletions(-) diff --git a/src/wt-assign3-2025-5-joint-state-publisher-pkg/CMakeLists.txt b/src/wt-assign3-2025-5-joint-state-publisher-pkg/CMakeLists.txt index 7c73ef9..94347fa 100644 --- a/src/wt-assign3-2025-5-joint-state-publisher-pkg/CMakeLists.txt +++ b/src/wt-assign3-2025-5-joint-state-publisher-pkg/CMakeLists.txt @@ -15,7 +15,6 @@ find_package(sensor_msgs REQUIRED) # set dependencies set(dependencies rclcpp -tf2_ros geometry_msgs sensor_msgs ) @@ -33,4 +32,4 @@ DIRECTORY launch DESTINATION share/${PROJECT_NAME} ) -ament_package() \ No newline at end of file +ament_package() diff --git a/src/wt-assign3-2025-5-joint-state-publisher-pkg/launch/inverse_kinematic.launch.xml b/src/wt-assign3-2025-5-joint-state-publisher-pkg/launch/inverse_kinematic.launch.xml index 4d2d188..ec1e24f 100644 --- a/src/wt-assign3-2025-5-joint-state-publisher-pkg/launch/inverse_kinematic.launch.xml +++ b/src/wt-assign3-2025-5-joint-state-publisher-pkg/launch/inverse_kinematic.launch.xml @@ -2,30 +2,11 @@ - - - - - - - - - - - - - - - - - - - - - + + diff --git a/src/wt-assign3-2025-5-joint-state-publisher-pkg/package.xml b/src/wt-assign3-2025-5-joint-state-publisher-pkg/package.xml index 6cbe252..1bebed6 100644 --- a/src/wt-assign3-2025-5-joint-state-publisher-pkg/package.xml +++ b/src/wt-assign3-2025-5-joint-state-publisher-pkg/package.xml @@ -9,7 +9,6 @@ ament_cmake rclcpp - tf2_ros geometry_msgs sensor_msgs diff --git a/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp b/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp index 1648782..2da1bba 100644 --- a/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp +++ b/src/wt-assign3-2025-5-joint-state-publisher-pkg/src/InverseKinematicNode.cpp @@ -1,301 +1,151 @@ -#include - #include -#include -#include #include #include class JointPublisherNode: public rclcpp::Node { public: - JointPublisherNode(): Node("ik_node") { - // PAROL6 link lengths from DH parameters (in meters) - a1_ = this->declare_parameter("a1", 0.11050); // Base height - a2_ = this->declare_parameter("a2", 0.02342); // Base offset - a3_ = this->declare_parameter("a3", 0.18000); // Shoulder to elbow - a4_ = this->declare_parameter("a4", 0.04350); // Elbow offset - a5_ = this->declare_parameter("a5", 0.17635); // Elbow to wrist - a6_ = this->declare_parameter("a6", 0.06280); // Wrist offset - a7_ = this->declare_parameter("a7", 0.04525); // End effector length - - // Connect 4 board parameters - cell_column_ = this->declare_parameter("cell_column", -1); // -1 = auto-cycle - cell_row_ = this->declare_parameter("cell_row", -1); // -1 = auto-cycle - board_x_ = this->declare_parameter("board_x", 0.25); - board_y_ = this->declare_parameter("board_y", 0.0); - board_z_ = this->declare_parameter("board_z", 0.15); - cell_spacing_ = this->declare_parameter("cell_spacing", 0.025); - case_number_ = this->declare_parameter("case_number", 0); + JointPublisherNode(): Node("parol6_ik_node") { + auto column_desc = rcl_interfaces::msg::ParameterDescriptor(); + column_desc.description = "Column selection (1-7) "; + column_desc.read_only = false; + column_desc.integer_range.resize(1); + column_desc.integer_range[0].from_value = 1; + column_desc.integer_range[0].to_value = 7; + column_desc.integer_range[0].step = 1; - calculateInverseKinematics(case_number_); + column_number_ = this->declare_parameter("column_number", 4, column_desc); + + // Connect4 board configuration + board_distance_ = this->declare_parameter("board_distance", 0.30); + board_z_ = this->declare_parameter("board_z", 0.30); + column_spacing_ = this->declare_parameter("column_spacing", 0.04); + board_width_ = 6 * column_spacing_; + + param_callback_handle_ = this->add_on_set_parameters_callback( + std::bind(&JointPublisherNode::callback_parameters, this, std::placeholders::_1) + ); + + calculate_target_position_from_column(); + calculate_3dpose(target_x_, target_y_, target_z_); joint_state_publisher_ = this->create_publisher("joint_states", 10); - // transform buffer and listener - tf_buffer_ = std::make_shared(this->get_clock()); - tf_listener_ = std::make_shared(*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) - // ); - - int current_col = 0; - int current_row = 0; - bool auto_cycle = (cell_column_ == -1 || cell_row_ == -1); - - while(true) { - // Use specific cell or auto-cycle through all cells - int col = auto_cycle ? current_col : cell_column_; - int row = auto_cycle ? current_row : cell_row_; - - // Clamp values to valid range - col = std::max(0, std::min(6, col)); - row = std::max(0, std::min(5, row)); - - // Calculate position for current cell (ready to drop coin) - // Board rotated 90 degrees: columns are along Y axis, rows along Z axis - // Position above top of column for coin drop - target_x_ = board_x_; // Fixed distance from robot - target_y_ = board_y_ + (col - 3) * cell_spacing_; // Column position (left to right) - target_z_ = board_z_ + 0.1; // Above the board to drop coins - - calculateInverseKinematics(case_number_); - broadcastJointState(); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); - - // Move to next column only in auto-cycle mode (cycle through columns, not all cells) - if (auto_cycle) { - current_col++; - if (current_col >= 7) { - current_col = 0; - } - } - } - - RCLCPP_INFO(this->get_logger(), - "Case %d: Joint states (%f %f %f %f %f %f) were calculated and sent to the system.", - case_number_, theta_1_, theta_2_, theta_3_, theta_4_, theta_5_, theta_6_ - ); - - // periodically broadcast joint state + // Periodically broadcast joint state timer_ = this->create_wall_timer( std::chrono::milliseconds(100), - std::bind(&JointPublisherNode::broadcastJointState, this) + std::bind(&JointPublisherNode::broadcast_joint_state, this) ); } private: + OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + rclcpp::TimerBase::SharedPtr timer_; rclcpp::Publisher::SharedPtr joint_state_publisher_; - rclcpp::TimerBase::SharedPtr timer_; - std::shared_ptr tf_buffer_; - std::shared_ptr tf_listener_; - OnSetParametersCallbackHandle::SharedPtr param_callback_handle_; + double a1_ = 0.11050; // base height + double a2_ = 0.02342; // base offset + double a3_ = 0.18000; // shoulder to elbow + double a4_ = 0.04350; // elbow offset + double a5_ = 0.17635; // elbow to wrist + double a6_ = 0.06280; // wrist offset + double a7_ = 0.04525; // end effector length - double target_x_; - double target_y_; - double target_z_; - // 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) - - // Connect 4 board parameters - int cell_column_; - int cell_row_; - double board_x_; - double board_y_; - double board_z_; - double cell_spacing_; - int case_number_; + double target_x_, target_y_, target_z_; + double theta_1_, theta_2_, theta_3_, theta_4_, theta_5_, theta_6_; - double theta_1_; - double theta_2_; - double theta_3_; - double theta_4_; - double theta_5_; - double theta_6_; + int column_number_; + double board_distance_, board_z_, board_width_, column_spacing_; - rcl_interfaces::msg::SetParametersResult parametersCallback( - const std::vector ¶meters) { + void calculate_target_position_from_column() { + // Column 1 is leftmost (-3 spacing), Column 4 is center (0), Column 7 is rightmost (+3 spacing) + // offset from center column + double column_offset = (column_number_ - 4.0) * column_spacing_; + // maintain consistent reach by calculating angle to column + // board is positioned at board_distance_ from robot base + double angle_to_column = std::atan2(column_offset, board_distance_); + + // target position in polar coordinates, then convert to cartesian + // ensuring consistent radial distance to all columns + target_x_ = board_distance_ * std::cos(angle_to_column); + target_y_ = board_distance_ * std::sin(angle_to_column); + target_z_ = board_z_; + + RCLCPP_INFO(this->get_logger(), + "column_number_=%d, position (x=%.3f, y=%.3f, z=%.3f), angle=%.2f rad", + column_number_, target_x_, target_y_, target_z_, angle_to_column + ); + } + + rcl_interfaces::msg::SetParametersResult callback_parameters( + const std::vector ¶meters + ) { + bool recalculate = false; rcl_interfaces::msg::SetParametersResult result; result.successful = true; - bool recalculate = false; - for (const auto ¶m : parameters) { - if (param.get_name() == "target_x") { - target_x_ = param.as_double(); + if (param.get_name() == "column_number") { + column_number_ = param.as_int(); recalculate = true; - RCLCPP_INFO(this->get_logger(), "Updated target_x to %f", target_x_); - } else if (param.get_name() == "target_y") { - 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() == "a1") { - a1_ = param.as_double(); - recalculate = true; - 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 a3 to %f", a3_); - } else if (param.get_name() == "a5") { - a5_ = param.as_double(); - recalculate = true; - RCLCPP_INFO(this->get_logger(), "Updated a5 to %f", a5_); - } else if (param.get_name() == "case_number") { - case_number_ = param.as_int(); - recalculate = true; - RCLCPP_INFO(this->get_logger(), "Updated case_number to %d", case_number_); + RCLCPP_INFO(this->get_logger(), "changed 'column_number' -> %d", column_number_); } } if (recalculate) { - calculateInverseKinematics(case_number_); - RCLCPP_INFO(this->get_logger(), - "Recalculated joint states: (%f %f %f %f %f %f)", - theta_1_, theta_2_, theta_3_, theta_4_, theta_5_, theta_6_); + calculate_target_position_from_column(); + calculate_3dpose(target_x_, target_y_, target_z_); } return result; } - void broadcastJointState() { + void broadcast_joint_state() { auto joint_state = sensor_msgs::msg::JointState(); joint_state.header.stamp = this->get_clock()->now(); - joint_state.name = { "L1", "L2", "L3", "L4", "L5", "L6" }; + joint_state.name = { "L1", "L2", "L3", "L4", "L5", "L6" }; joint_state.position = { theta_1_, theta_2_, theta_3_, theta_4_, theta_5_, theta_6_ }; joint_state_publisher_->publish(joint_state); } - void calculateInverseKinematics(int case_number) { - double x, y, z; + void calculate_3dpose(double x, double y, double z) { + RCLCPP_INFO(this->get_logger(), "target position (x=%.3f, y=%.3f, z=%.3f)", x, y, z); - // 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 || 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 { - // Default test positions for different cases - switch(case_number) { - case 1: - x = 0.20; - y = 0.10; - z = 0.15; - break; - case 2: - x = 0.15; - y = 0.15; - z = 0.20; - break; - case 3: - x = 0.18; - y = 0.05; - z = 0.18; - break; - case 4: - x = 0.12; - y = 0.12; - z = 0.15; - break; - default: - x = 0.15; - y = 0.10; - z = 0.15; - 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, z=%f", x, y, z); - - // PAROL6 Inverse Kinematics Solution - // Based on DH parameters and kinematic structure - - // Joint 1 (Base rotation) - simple atan2 for rotation around Z axis - theta_1_ = std::atan2(y, x); - - // 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 + // distance from base + offset + double r = std::sqrt(x*x + y*y); double r_adjusted = r - a2_; - // Height from shoulder joint (which is at height a1) + // joint 1 (base): atan2 for rotation around z axis + theta_1_ = std::atan2(y, x); + + // height from shoulder joint 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); + double d = std::sqrt(r_adjusted*r_adjusted + z_from_shoulder*z_from_shoulder); - // 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; - } - - // Joint 3 (Elbow) - law of cosines - // Use negative angle for "elbow down" configuration to allow arm extension + // Joint 3 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 + // Clamp to [-1, 1] + cos_theta_3 = std::max(-1.0, std::min(1.0, cos_theta_3)); + theta_3_ = -std::acos(cos_theta_3); - // Joint 2 (Shoulder) - geometric solution - double alpha = std::atan2(wrist_z, wrist_r); // Angle to target + // Joint 2 + double alpha = std::atan2(z_from_shoulder, r_adjusted); double beta = std::atan2(a5_ * std::sin(-theta_3_), a3_ + a5_ * std::cos(-theta_3_)); theta_2_ = alpha - beta; - // Wrist joints - add some motion for variety - // theta_4: Slowly oscillating wrist pitch - // theta_5: Varies with base rotation for coordinated movement - // theta_6: Continuous slow rotation - static double time_counter = 0.0; - time_counter += 0.01; - theta_4_ = 0.3 * std::sin(time_counter * 0.5); - theta_5_ = 0.4 * std::sin(time_counter * 0.7 + theta_1_); - theta_6_ = time_counter * 0.1; + // wrist joints + theta_4_ = 0; + theta_5_ = 0; + theta_6_ = 0.0; RCLCPP_INFO(this->get_logger(), - "Calculated angles - J1(base): %f, J2(shoulder): %f, J3(elbow): %f", - theta_1_, theta_2_, theta_3_); + "joint angles (J1=%.3f, J2=%.3f, J3=%.3f, J4=%.3f, J5=%.3f, J6=%.3f)", + theta_1_, theta_2_, theta_3_, theta_4_, theta_5_, theta_6_ + ); } };