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

feat: Working connect4 IK

Browse Source
This commit is contained in:
Wessel T
2025-12-13 21:43:43 +01:00
parent 6a6a9c9dcf
commit c951c9e537
4 changed files with 93 additions and 264 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
src/wt-assign3-2025-5-joint-state-publisher-pkg/CMakeLists.txt
View File

@@ -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()
ament_package()

23
src/wt-assign3-2025-5-joint-state-publisher-pkg/launch/inverse_kinematic.launch.xml
View File

@@ -2,30 +2,11 @@
<let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/urdf/robot_with_game.urdf.xacro"/>
<let name="rviz_config_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/rviz/urdf_config.rviz"/>
<!-- Connect 4 cell position parameters -->
<arg name="cell_column" default="-1" description="Connect 4 column (0-6, -1 for auto-cycle)"/>
<arg name="cell_row" default="-1" description="Connect 4 row (0-5, -1 for auto-cycle)"/>
<arg name="board_x" default="0.30" description="Board center X position"/>
<arg name="board_y" default="0.0" description="Board center Y position"/>
<arg name="board_z" default="0.30" description="Height above cells"/>
<arg name="cell_spacing" default="0.04" description="Distance between cell centers"/>
<arg name="case_number" default="0" description="Case number for inverse kinematics test"/>
<node pkg="robot_state_publisher" exec="robot_state_publisher">
<param name="robot_description" value="$(command 'xacro $(var urdf_path)')" />
</node>
<node pkg="wt-assign3-2025-5-joint-state-publisher-pkg" exec="ik_node">
<param name="cell_column" value="$(var cell_column)"/>
<param name="cell_row" value="$(var cell_row)"/>
<param name="board_x" value="$(var board_x)"/>
<param name="board_y" value="$(var board_y)"/>
<param name="board_z" value="$(var board_z)"/>
<param name="cell_spacing" value="$(var cell_spacing)"/>
<param name="case_number" value="$(var case_number)"/>
</node>
<!--<node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui" />-->
<node pkg="wt-assign3-2025-5-joint-state-publisher-pkg" exec="ik_node" />
<node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)" />
<node pkg="rqt_reconfigure" exec="rqt_reconfigure" output="screen" />
</launch>

1
src/wt-assign3-2025-5-joint-state-publisher-pkg/package.xml
View File

@@ -9,7 +9,6 @@
<buildtool_depend>ament_cmake</buildtool_depend>
<depend>rclcpp</depend>
<depend>tf2_ros</depend>
<depend>geometry_msgs</depend>
<depend>sensor_msgs</depend>

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

@@ -1,301 +1,151 @@
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/buffer.h>
#include <sensor_msgs/msg/joint_state.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
class JointPublisherNode: public rclcpp::Node {
public:
JointPublisherNode(): Node("ik_node") {
// 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
// Connect 4 board parameters
cell_column_ = this->declare_parameter<int>("cell_column", -1); // -1 = auto-cycle
cell_row_ = this->declare_parameter<int>("cell_row", -1); // -1 = auto-cycle
board_x_ = this->declare_parameter<double>("board_x", 0.25);
board_y_ = this->declare_parameter<double>("board_y", 0.0);
board_z_ = this->declare_parameter<double>("board_z", 0.15);
cell_spacing_ = this->declare_parameter<double>("cell_spacing", 0.025);
case_number_ = this->declare_parameter<int>("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<int>("column_number", 4, column_desc);
// Connect4 board configuration
board_distance_ = this->declare_parameter<double>("board_distance", 0.30);
board_z_ = this->declare_parameter<double>("board_z", 0.30);
column_spacing_ = this->declare_parameter<double>("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<sensor_msgs::msg::JointState>("joint_states", 10);
// transform buffer and listener
tf_buffer_ = std::make_shared<tf2_ros::Buffer>(this->get_clock());
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)
// );
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<sensor_msgs::msg::JointState>::SharedPtr joint_state_publisher_;
rclcpp::TimerBase::SharedPtr timer_;
std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
std::shared_ptr<tf2_ros::TransformListener> 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<rclcpp::Parameter> &parameters) {
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<rclcpp::Parameter> &parameters
) {
bool recalculate = false;
rcl_interfaces::msg::SetParametersResult result;
result.successful = true;
bool recalculate = false;
for (const auto &param : 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_
);
}
};