RTT/ROS/KDL Tools¶
Available at https://github.com/kuka-isir/rtt_ros_kdl_tools
This set of tools helps you build and use a robot kinematic model via a provided URDF.
Robot Kinematic Model¶
The ChainUtils class provides tools to compute forward kinematics, external torques etc. It devrives from ChainUtilsBase and adds non-realtime inverse kinematics with trac_ik.
Frames of reference / ROS params¶
To build the kinematic chain, we’ll use the folowing ros params :
/robot_description
(the URDF uploaded)/root_link
(default/recommended : /link_0)/tip_link
(example : /ati_link)
All those parameters must live in the relative namespace (http://wiki.ros.org/Names) from your node to build the chain with KDL.
Note
If you use the rtt_lwr
tools, like the generated run.launch
linked to lwr_utils
, the params are automatically sent when you’ll run your controller.
Build your ChainUtils model¶
In your package.xml
:
<build_depend>rtt_ros_kdl_tools</build_depend>
<run_depend>rtt_ros_kdl_tools</run_depend>
In your CMakeLists.txt
:
# Add the rtt_ros dependency
find_package(catkin REQUIRED COMPONENTS rtt_ros)
# It's gonna import all orocos libraries in the package.xml
In your .hpp
:
#include <rtt_ros_kdl_tools/chain_utils.hpp>
In your .cpp
:
// Construct your model
rtt_ros_kdl_tools::ChainUtils arm;
// Initialise chain from robot_description (via ROS Params)
// It reads /robot_description
// /tip_link
// /root_link
arm.init();
Internal Model¶
You need to tell ChainUtils
the state of the robot and compute some internal model variables.
// Ex : read from orocos port in an Eigen::VectorXd
port_joint_position_in.read(jnt_pos_in); // Check if RTT::NewData
port_joint_velocity_in.read(jnt_vel_in); // Check if RTT::NewData
// Feed the internal state
arm.setState(jnt_pos_in,jnt_vel_in);
// Update the internal model
arm.updateModel();
Forward kinematics¶
// Ex : read from orocos port in an Eigen::VectorXd
port_joint_position_in.read(jnt_pos_in); // Check if RTT::NewData
port_joint_velocity_in.read(jnt_vel_in); // Check if RTT::NewData
// Feed the internal state
arm.setState(jnt_pos_in,jnt_vel_in);
// Update the internal model
arm.updateModel();
// Ex : get a specific segment position
KDL::Frame& X = arm.getSegmentPosition("link_7");
// Ex : get the 5th segment
KDL::Frame& X = arm.getSegmentPosition(5);
// Get Root Link
std::string root_link = arm.getRootSegmentName();
Inverse kinematics with trac_ik¶
// Ex : read from orocos port in an Eigen::VectorXd
port_joint_position_in.read(jnt_pos_in); // Check if RTT::NewData
port_joint_velocity_in.read(jnt_vel_in); // Check if RTT::NewData
// Feed the internal state
arm.setState(jnt_pos_in,jnt_vel_in);
// Update the internal model
arm.updateModel();
// Transform it to a KDL JntArray
KDL::JntArray joint_seed(jnt_pos_in.size());
joint_seed.data = jnt_pos_in;
// Allocate the output of the Inverse Kinematics
KDL::JntArray return_joints(jnt_pos_in.size());
// Create an desired frame
KDL::Frame desired_end_effector_pose(
KDL::Rotation::RPY(-1.57,0,1.57), // Rotation rad
KDL::Vector(-0.2,-0.3,0.8)); // Position x,y,z in meters
// Define some tolerances
KDL::Twist tolerances(
KDL::Vector(0.01,0.01,0.01), // Tolerance x,y,z in meters
KDL::Vector(0.01,0.01,0.01)); // Tolerance Rx,Rz,Rz in rad
// Call the inverse function
if(arm.cartesianToJoint(joint_seed,
desired_end_effector_pose,
return_joints,
tolerances))
{
log(Debug) << "Success ! Result : "
<<return_joints.data.transpose() << endlog();
}
Warning
trac-ik
is not realtime-safe and therefore should not be used in updateHook()