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_
+ );
}
};