Loading files
This file is pinned to the selected release. Download the original to open it in your design tool.
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <kalman_interfaces/msg/arm_values.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/server_goal_handle.hpp>
#include <rclcpp_components/register_node_macro.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <kdl/chain.hpp>
#include <kdl/chainiksolvervel_wdls.hpp>
#include <kdl/jntarray.hpp>
#include <kdl/tree.hpp>
#include <kdl_parser/kdl_parser.hpp>
#include <std_msgs/msg/string.hpp>
#include <urdf/model.h>
namespace kalman_arm2 {
class TwistIK : public rclcpp::Node {
public:
// ROS interfaces
rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub;
rclcpp::Subscription<kalman_interfaces::msg::ArmValues>::SharedPtr
joint_pos_sub;
rclcpp::Subscription<std_msgs::msg::String>::SharedPtr
robot_description_sub;
rclcpp::Publisher<kalman_interfaces::msg::ArmValues>::SharedPtr
joint_vel_pub;
rclcpp::TimerBase::SharedPtr compute_timer;
// TF2
std::shared_ptr<tf2_ros::TransformListener> tf_listener;
std::shared_ptr<tf2_ros::Buffer> tf_buffer;
// Parameters
std::string base_link;
std::string end_effector_link;
float max_joint_vel;
double update_rate;
double control_timeout;
// Kinematics
KDL::Chain arm_chain;
std::unique_ptr<KDL::ChainIkSolverVel_wdls> ik_solver;
KDL::JntArray current_joint_positions;
bool joints_initialized;
bool kinematics_ready;
// Latest twist message
geometry_msgs::msg::TwistStamped::SharedPtr latest_twist;
std::mutex twist_mutex;
rclcpp::Time last_twist_time;
TwistIK(const rclcpp::NodeOptions &options)
: Node("twist_ik", options), joints_initialized(false),
kinematics_ready(false), last_twist_time(now()) {
this->declare_parameter<std::string>("base_link", "base_link");
this->declare_parameter<std::string>(
"end_effector_link", "arm_link_end"
);
this->declare_parameter<float>("max_joint_vel", 0.5);
this->declare_parameter<double>("update_rate", 10.0);
this->declare_parameter<double>("control_timeout", 0.5);
this->get_parameter("base_link", base_link);
this->get_parameter("end_effector_link", end_effector_link);
this->get_parameter("max_joint_vel", max_joint_vel);
this->get_parameter("update_rate", update_rate);
this->get_parameter("control_timeout", control_timeout);
// Initialize TF2
tf_buffer = std::make_shared<tf2_ros::Buffer>(get_clock());
tf_listener = std::make_shared<tf2_ros::TransformListener>(*tf_buffer);
// Publishers & subscribers
joint_vel_pub = create_publisher<kalman_interfaces::msg::ArmValues>(
"target_vel", 10
);
joint_pos_sub = create_subscription<kalman_interfaces::msg::ArmValues>(
"current_pos",
10,
std::bind(&TwistIK::on_joint_positions, this, std::placeholders::_1)
);
twist_sub = create_subscription<geometry_msgs::msg::TwistStamped>(
"target_twist",
10,
std::bind(&TwistIK::on_target_twist, this, std::placeholders::_1)
);
// Create timer for periodic computation
auto timer_period = std::chrono::duration<double>(1.0 / update_rate);
compute_timer = create_wall_timer(
timer_period, std::bind(&TwistIK::compute_joint_velocities, this)
);
// QoS profile for robot_description (latched topic)
rclcpp::QoS robot_description_qos(1);
robot_description_qos.transient_local();
robot_description_qos.reliable();
robot_description_sub = create_subscription<std_msgs::msg::String>(
"/robot_description",
robot_description_qos,
std::bind(
&TwistIK::on_robot_description, this, std::placeholders::_1
)
);
RCLCPP_INFO(
get_logger(),
"TwistIK node waiting for robot description on "
"/robot_description..."
);
}
void on_robot_description(const std_msgs::msg::String::SharedPtr msg) {
if (kinematics_ready) {
return;
}
urdf::Model model;
if (!model.initString(msg->data)) {
RCLCPP_ERROR(
get_logger(), "Failed to parse URDF from /robot_description."
);
return;
}
KDL::Tree kdl_tree;
if (!kdl_parser::treeFromUrdfModel(model, kdl_tree)) {
RCLCPP_ERROR(get_logger(), "Failed to create KDL tree from URDF.");
return;
}
if (!kdl_tree.getChain(base_link, end_effector_link, arm_chain)) {
RCLCPP_ERROR(
get_logger(),
"Failed to extract KDL chain from '%s' to '%s'.",
base_link.c_str(),
end_effector_link.c_str()
);
return;
}
// init joint weights
size_t nj = arm_chain.getNrOfJoints();
Eigen::MatrixXd joint_weights = Eigen::MatrixXd::Identity(nj, nj);
// 4th joint often flips 180 degrees when 5th goes through zero, let's
// dampen it
joint_weights(3, 3) = 10.0;
joint_weights(4, 4) = 0.5;
ik_solver = std::make_unique<KDL::ChainIkSolverVel_wdls>(arm_chain);
ik_solver->setWeightJS(joint_weights);
ik_solver->setLambda(0.5);
current_joint_positions.resize(arm_chain.getNrOfJoints());
kinematics_ready = true;
RCLCPP_INFO(
get_logger(),
"Kinematic chain initialized with %d joints.",
arm_chain.getNrOfJoints()
);
}
void
on_joint_positions(const kalman_interfaces::msg::ArmValues::SharedPtr msg) {
if (!kinematics_ready) {
return;
}
if (msg->joints.size() < current_joint_positions.rows()) {
RCLCPP_WARN(
get_logger(),
"Received joint position size (%ld) < expected (%d).",
msg->joints.size(),
current_joint_positions.rows()
);
return;
}
for (size_t i = 0; i < current_joint_positions.rows(); ++i) {
current_joint_positions(i) = msg->joints[i];
}
joints_initialized = true;
}
geometry_msgs::msg::Twist transform_twist_to_base_frame(
const geometry_msgs::msg::TwistStamped::SharedPtr &msg
) {
geometry_msgs::msg::Twist base_twist = msg->twist;
// Transform twist to base frame if frame_id is provided and not empty
if (!msg->header.frame_id.empty() &&
msg->header.frame_id != base_link) {
try {
// Get transform from source frame to base frame
geometry_msgs::msg::TransformStamped transform =
tf_buffer->lookupTransform(
base_link,
msg->header.frame_id,
msg->header.stamp,
rclcpp::Duration::from_nanoseconds(100000000)
); // 100ms timeout
auto q = tf2::Quaternion(
transform.transform.rotation.x,
transform.transform.rotation.y,
transform.transform.rotation.z,
transform.transform.rotation.w
)
.normalize();
// Transform linear velocity
tf2::Vector3 lin(
msg->twist.linear.x,
msg->twist.linear.y,
msg->twist.linear.z
);
tf2::Vector3 lin_base = tf2::quatRotate(q, lin);
// Transform angular velocity
tf2::Vector3 ang(
msg->twist.angular.x,
msg->twist.angular.y,
msg->twist.angular.z
);
tf2::Vector3 ang_base = tf2::quatRotate(q, ang);
// ^ Note: Angular velocity is an rvec, it can be safely
// transformed like this
base_twist.linear.x = lin_base.x();
base_twist.linear.y = lin_base.y();
base_twist.linear.z = lin_base.z();
base_twist.angular.x = ang_base.x();
base_twist.angular.y = ang_base.y();
base_twist.angular.z = ang_base.z();
RCLCPP_DEBUG(
get_logger(),
"Transformed twist from frame '%s' to '%s'",
msg->header.frame_id.c_str(),
base_link.c_str()
);
} catch (const tf2::TransformException &ex) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"Could not transform twist from '%s' to '%s': %s. Using "
"twist as-is.",
msg->header.frame_id.c_str(),
base_link.c_str(),
ex.what()
);
}
}
return base_twist;
}
void
on_target_twist(const geometry_msgs::msg::TwistStamped::SharedPtr msg) {
std::lock_guard<std::mutex> lock(twist_mutex);
latest_twist = msg;
last_twist_time = now();
}
void compute_joint_velocities() {
if (!kinematics_ready || !joints_initialized) {
return;
}
geometry_msgs::msg::TwistStamped::SharedPtr twist_msg;
{
std::lock_guard<std::mutex> lock(twist_mutex);
if (!latest_twist ||
(now() - last_twist_time).seconds() > control_timeout) {
return;
}
twist_msg = latest_twist;
}
geometry_msgs::msg::Twist base_twist =
transform_twist_to_base_frame(twist_msg);
KDL::Twist target_twist;
target_twist.vel.x(base_twist.linear.x);
target_twist.vel.y(base_twist.linear.y);
target_twist.vel.z(base_twist.linear.z);
target_twist.rot.x(base_twist.angular.x);
target_twist.rot.y(base_twist.angular.y);
target_twist.rot.z(base_twist.angular.z);
KDL::JntArray joint_velocities(current_joint_positions.rows());
int result = ik_solver->CartToJnt(
current_joint_positions, target_twist, joint_velocities
);
if (result < 0) {
RCLCPP_WARN_THROTTLE(
get_logger(),
*get_clock(),
1000,
"IK velocity solver failed (code %d)",
result
);
return;
}
// Scale joint velocities to fit within max_joint_vel
float max_computed_vel = joint_velocities.data.maxCoeff();
float scale = max_joint_vel / max_computed_vel;
if (max_computed_vel > max_joint_vel) {
for (size_t i = 0; i < joint_velocities.rows(); ++i) {
joint_velocities(i) *= scale;
}
RCLCPP_DEBUG(
get_logger(),
"Scaling joint velocities by %.2f to fit within %.2f",
scale,
max_joint_vel
);
}
// Translate to a joint velocity message
auto vel_msg = kalman_interfaces::msg::ArmValues();
vel_msg.header.stamp = now();
for (size_t i = 0; i < joint_velocities.rows(); ++i) {
vel_msg.joints[i] = joint_velocities(i);
}
joint_vel_pub->publish(vel_msg);
}
};
} // namespace kalman_arm2
RCLCPP_COMPONENTS_REGISTER_NODE(kalman_arm2::TwistIK)
d1a09a6b3This file is pinned to the selected release. Download the original to open it in your design tool.