(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Introduction to the KDL Manager

Description: How to initialize the KDL manager and load a kinematic chain's information from the ROS parameter server.

Keywords: KDL, KDLManager, generic_control_toolbox, control

Tutorial Level: BEGINNER

Next Tutorial: Basic control algorithm

Introduction

The KDLManager class encapsulates useful methods for handling KDL when implementing a robot controller in ROS. In this tutorial, we will guide you through the process of building a ROS package which uses the KDLManager to load and manage a kinematic chain.

Setting up the workspace

We will need the generic_control_toolbox and a robot description to be available for this tutorial. We will use the Baxter common package to provide a robot description.

$ mkdir -p ~/catkin_ws/src && cd ~/catkin_ws/src
$ catkin_init_workspace
$ git clone https://github.com/diogoalmeida/generic_control_toolbox.git
$ git clone https://github.com/RethinkRobotics/baxter_common.git

And make a new package

$ mkdir -p kdl_manager_tutorial/src

The code

We will create a ROS node which initializes a KDL Manager instance and, from available JointState information, compute the end-effector and a configured gripping point poses of Baxter's left arm.

First, move to the source folder

$ cd kdl_manager_tutorial/src

and create a CPP file named 'load_chain.cpp' with the following code

https://raw.githubusercontent.com/diogoalmeida/generic_control_toolbox_tutorials/master/kdl_manager_tutorial/src/load_chain.cpp

   1 #include <ros/ros.h>
   2 #include <generic_control_toolbox/kdl_manager.hpp>
   3 
   4 
   5 int main (int argc, char ** argv)
   6 {
   7   ros::init(argc, argv, "load_chain");
   8 
   9   // Initialize a KDL manager instance with the 'torso' link configured as its
  10   // base frame.
  11   generic_control_toolbox::KDLManager manager("torso");
  12 
  13   // Load a kinematic chain which goes from the base frame ('torso') to a
  14   // user-configured end-effector ('left_hand')
  15   manager.initializeArm("left_hand");
  16   KDL::Frame pose;
  17   sensor_msgs::JointState state;
  18 
  19   // The KDL manager parses the robot state information from a JointState message,
  20   // which robots using ROS will publish on the network.
  21   state = *ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states");
  22 
  23   // Get the end-effector pose as a KDL::Frame.
  24   manager.getEefPose("left_hand", state, pose);
  25   ROS_INFO_STREAM("The left hand end-effector position is " << pose.p.x() << ", " << pose.p.y() << ", " << pose.p.z());
  26 
  27   // Configure a gripping point on the robot
  28   manager.setGrippingPoint("left_hand", "left_gripper");
  29 
  30   // Get the pose of the gripping point
  31   manager.getGrippingPoint("left_hand", state, pose);
  32   ROS_INFO_STREAM("The left hand gripping point position is " << pose.p.x() << ", " << pose.p.y() << ", " << pose.p.z());
  33 }

The Code Explained

We will break down the Manager's calls

  11   generic_control_toolbox::KDLManager manager("torso");

On construction, a KDL Manager receives the robot's base frame name, from which the kinematic chains will spawn.

  15   manager.initializeArm("left_hand");
  16   KDL::Frame pose;
  17   sensor_msgs::JointState state;

The KDL manager maintains a set of kinematic chains. Here, we load the chain that goes from our configured base frame ('torso') to the declared end-effector ('left_hand'). We additionally initialize the data structures needed to get kinematic information from a chain, a KDL Frame to store the pose information and a sensor_msgs JointState message to hold the robots state information.

  21   state = *ros::topic::waitForMessage<sensor_msgs::JointState>("/joint_states");
  22 
  23   // Get the end-effector pose as a KDL::Frame.
  24   manager.getEefPose("left_hand", state, pose);

We obtain the current robot state information from the joint_states topic and used the getEefPose method to compute the forward kinematics of the loaded joint chain.

  28   manager.setGrippingPoint("left_hand", "left_gripper");
  29 
  30   // Get the pose of the gripping point
  31   manager.getGrippingPoint("left_hand", state, pose);

Finally, we set the 'left_hand' frame to be a configured gripping point and compute its pose. Any TF frame can be set as a gripping point. Note: The manager assumes that the relationship between the end-effector frame and the gripping point frame is static.

Building the node

In the package root, add the following CMakeLists.txt file:

https://raw.githubusercontent.com/diogoalmeida/generic_control_toolbox_tutorials/master/kdl_manager_tutorial/CMakeLists.txt

   1 cmake_minimum_required(VERSION 2.8.3)
   2 project(kdl_manager_tutorial)
   3 
   4 find_package(
   5   catkin REQUIRED COMPONENTS
   6   roscpp
   7   generic_control_toolbox
   8 )
   9 
  10 add_definitions(-std=c++11)
  11 link_directories(${catkin_LIBRARY_DIRS})
  12 
  13 catkin_package(
  14   CATKIN_DEPENDS roscpp generic_control_toolbox
  15 )
  16 
  17 include_directories(
  18   ${catkin_INCLUDE_DIRS}
  19 )
  20 
  21 add_executable(load_chain src/load_chain.cpp)
  22 target_link_libraries(load_chain ${catkin_LIBRARIES})
  23 add_dependencies(load_chain ${catkin_EXPORTED_TARGETS})

And in your package.xml file do not forget to include

  ...
  <depend>roscpp</depend>
  <depend>generic_control_toolbox</depend>
  ...

Running your code

To use the KDL manager we need a joint_states message to be available through a ROS topic. This is the case when using a real robot or proper simulation. For the purposes of this tutorial, we will use Baxter's description and the 'joint_state_publisher' package to generate a joint_states message. Make a 'launch' directory

$ mkdir launch

And add a 'tutorial.launch' launch file

<launch>
  <param name="robot_description" command="$(find xacro)/xacro.py --inorder $(find baxter_description)/urdf/baxter.urdf.xacro gazebo:=false"/>
  <node pkg="kdl_manager_tutorial" type="load_chain" name="load_chain" output="screen"/>
  <node pkg="joint_state_publisher" type="joint_state_publisher" name="joint_state_publisher">
    <param name="publish_default_velocities" type="bool" value="true" />
  </node>
</launch>

and, after compiling and sourcing your workspace, the code can now be tested with

$ roslaunch kdl_manager_tutorial tutorial.launch

Wiki: generic_control_toolbox/Tutorials/KDL Manager (last edited 2019-07-26 08:30:09 by DiogoAlmeida)