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

feat: First kinematics version

Browse Source
This commit is contained in:
Wessel T
2025-11-27 11:33:29 +01:00
parent ec381be993
commit 6cb924e8e9
2 changed files with 210 additions and 73 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,10 +1,27 @@
<launch>
<let name="urdf_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/urdf/PAROL6.urdf.xacro"/>
<let name="rviz_config_path" value="$(find-pkg-share wt-assign1-2025-4-parol6-static-pkg)/rviz/urdf_config.rviz"/>
<!-- Declare case number argument with default value -->
<arg name="L1" default="0.18" description="Length of first link"/>
<arg name="L2" default="0.0435" description="Length of second link"/>
<arg name="target_x" default="0.0" description="Target x coordinate"/>
<arg name="target_y" default="0.0" description="Target y coordinate"/>
<arg name="case_number" default="2" 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" />
<node pkg="wt-assign3-2025-5-joint-state-publisher-pkg" exec="ik_node">
<param name="L1" value="$(var L1)"/>
<param name="L2" value="$(var L2)"/>
<param name="target_x" value="$(var target_x)"/>
<param name="target_y" value="$(var target_y)"/>
<param name="case_number" value="$(var case_number)"/>
</node>
<!--<node pkg="joint_state_publisher_gui" exec="joint_state_publisher_gui" />-->
<node pkg="rviz2" exec="rviz2" output="screen" args="-d $(var rviz_config_path)" />
</launch>
</launch>

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

@@ -1,78 +1,198 @@
#include <cmath>
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/joint_state.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") {
//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<double>("L1", 0.18);
L2_ = this->declare_parameter<double>("L2", 0.0435);
target_x_ = this->declare_parameter<double>("target_x", 0.0);
target_y_ = this->declare_parameter<double>("target_y", 0.0);
case_number_ = this->declare_parameter<int>("case_number", 0);
calculateInverseKinematics(case_number_);
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)
);
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;
}
private:
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 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<rclcpp::Parameter> &parameters) {
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();
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<JointPublisherNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}