From 6cb924e8e93fa3c2a92192f154b9a2aabd6669b6 Mon Sep 17 00:00:00 2001 From: Wessel Tip Date: Thu, 27 Nov 2025 11:33:29 +0100 Subject: [PATCH] feat: First kinematics version --- .../launch/inverse_kinematic.launch.xml | 21 +- .../src/InverseKinematicNode.cpp | 262 +++++++++++++----- 2 files changed, 210 insertions(+), 73 deletions(-) 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 f2c1672..83faae9 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 @@ -1,10 +1,27 @@ + + + + + + + + - + + + + + + + + + + - \ No newline at end of file + 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 43ca908..d02f281 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,78 +1,198 @@ +#include + #include - -#include - #include - #include - +#include #include class JointPublisherNode: public rclcpp::Node { - public: JointPublisherNode(): Node("ik_node") { - //Declare and get parameters - this -> declare_parameter < double > ("theta_1", 0.0); - this -> declare_parameter < double > ("theta_2", 0.0); - this -> declare_parameter < double > ("theta_3", 0.0); - this -> declare_parameter < double > ("theta_4", 0.0); - this -> declare_parameter < double > ("theta_5", 0.0); - this -> declare_parameter < double > ("theta_6", 0.0); - theta_1_ = this -> get_parameter("theta_1").as_double(); - theta_2_ = this -> get_parameter("theta_2").as_double(); - theta_3_ = this -> get_parameter("theta_3").as_double(); - theta_4_ = this -> get_parameter("theta_4").as_double(); - theta_5_ = this -> get_parameter("theta_5").as_double(); - theta_6_ = this -> get_parameter("theta_6").as_double(); - // Initialize the joint state publisher - joint_state_publisher_ = this -> create_publisher < sensor_msgs::msg::JointState > ("joint_states", 10); - // Initialize the 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_); - RCLCPP_INFO(this -> get_logger(), "Joint states (%f %f %f %f %f %f) were sent to the system.", theta_1_, theta_2_, theta_3_, theta_4_, theta_5_, theta_6_); - // Set up a timer to periodically check for the transform and broadcast it - timer_ = this -> create_wall_timer( - std::chrono::milliseconds(100), - std::bind( & JointPublisherNode::broadcastJointState, this)); +public: + JointPublisherNode(): Node("ik_node") { + L1_ = this->declare_parameter("L1", 0.18); + L2_ = this->declare_parameter("L2", 0.0435); + target_x_ = this->declare_parameter("target_x", 0.0); + target_y_ = this->declare_parameter("target_y", 0.0); + case_number_ = this->declare_parameter("case_number", 0); + + calculateInverseKinematics(case_number_); + + 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) + ); + + 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 + timer_ = this->create_wall_timer( + std::chrono::milliseconds(100), + std::bind(&JointPublisherNode::broadcastJointState, this) + ); } - private: void broadcastJointState() { - // Create a JointState message - auto joint_state = sensor_msgs::msg::JointState(); - joint_state.header.stamp = this -> get_clock() -> now(); - joint_state.name = { - "L1", // x - "L2", // x - "L3", // x - "L4", - "L5", // x - "L6" - }; - joint_state.position = { - theta_1_, - theta_2_, - theta_3_, - theta_4_, - theta_5_, - theta_6_ - }; - // Publish the joint state - joint_state_publisher_ -> publish(joint_state); - } - double theta_1_; - double theta_2_; - double theta_3_; - double theta_4_; - double theta_5_; - double theta_6_; - rclcpp::Publisher < sensor_msgs::msg::JointState > ::SharedPtr - joint_state_publisher_; - std::shared_ptr < tf2_ros::Buffer > tf_buffer_; - std::shared_ptr < tf2_ros::TransformListener > tf_listener_; - rclcpp::TimerBase::SharedPtr timer_; - }; - int main(int argc, char ** argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared < JointPublisherNode > (); - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; - } \ No newline at end of file + +private: + 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 target_x_; + double target_y_; + double L1_; + double L2_; + int case_number_; + + double theta_1_; + double theta_2_; + double theta_3_; + double theta_4_; + double theta_5_; + double theta_6_; + + rcl_interfaces::msg::SetParametersResult parametersCallback( + const std::vector ¶meters) { + + 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(); + 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() == "L1") { + L1_ = 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(); + recalculate = true; + RCLCPP_INFO(this->get_logger(), "Updated L2 to %f", L2_); + } 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_); + } + } + + 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_); + } + + return result; + } + + void broadcastJointState() { + 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.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; + + // Use target_x_ and target_y_ if explicitly set via parameters + // Otherwise use predefined case positions + bool use_target_params = (target_x_ != 0.0 || target_y_ != 0.0); + + if (use_target_params) { + x = target_x_; + y = target_y_; + RCLCPP_INFO(this->get_logger(), "Using explicit target position"); + } else { + switch(case_number) { + case 1: + x = 0.20; + y = 0.10; + break; + case 2: + x = 0.15; + y = 0.15; + break; + case 3: + x = 0.18; + y = 0.05; + break; + case 4: + x = 0.12; + y = 0.12; + break; + default: + x = 0.15; + y = 0.10; + 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); + + // theta_2 (law of cosines) + double cos_theta_2 = (x*x + y*y - 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"); + 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; + } + + double sin_theta_2 = std::sqrt(1 - cos_theta_2 * cos_theta_2); + double theta_2 = std::atan2(sin_theta_2, cos_theta_2); + + double k1 = L1_ + L2_ * cos_theta_2; + double k2 = L2_ * sin_theta_2; + + double gamma = std::atan2(k2, k1); + double theta_1 = std::atan2(y, x) - gamma; + + // 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_6_ = 0.0091; // L6 joint (fixed) + + RCLCPP_INFO(this->get_logger(), "Calculated angles - theta_1: %f, theta_2: %f", theta_1, theta_2); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +}