About | Support | Discussion Forum | Index | Service Status | ros @ Robotics Stack Exchange

Implementing a realtime Cartesian controller

Description: This tutorial teaches you how to control a robot in Cartesian space

Tutorial Level: ADVANCED

Next Tutorial: See the code examples in Coding a realtime Cartesian controller with KDL and Coding a realtime Cartesian controller with Eigen

Introduction

This tutorial teaches you how to implement a realtime controller that works in Cartesian space. It leverages both the contents and the code examples of the previous tutorials on writing and running a realtime joint controller.

Compared to individual joint control, writing a Cartesian controller brings up the additional needs to handle sets of joints, perform kinematic calculations, and execute linear algebra. We assume you understand, at least in concept, the ideas of a Jacobian matrix, kinematic chains, tip linear and angular velocities, etc.

For the kinematic calculations, ROS includes support of the Kinematics and Dynamics Library KDL (KDL homepage) via the KDL package in the geometry stack. KDL provides several powerful features. It

1. defines and understands (open) kinematic chains, so knowledge of the kinematic parameters (lengths, offsets, etc.) is sufficient to perform all calculations.

2. provides "solvers" to execute these calculations. For example calculating the tip position via forward kinematics or calculating the Jacobian matrix. These solvers are realtime safe functions, i.e. can be called from a realtime update loop.

3. declares explicit types for the necessary vectors and matrices:

   • KDL::JntArray for an Nx1 vector of joint quantities (positions, velocities, accelerations, or torques)

   • KDL::Frame for a Cartesian frame, i.e. position (3x1 vector) and orientation (3x3 rotation matrix)

   • KDL::Twist for a 6x1 vector of translational and orientational velocity

   • KDL::Wrench for a 6x1 vector of forces and torques

   • KDL::Jacobian for a 6xN Jacobian matrix

4. uses dynamically sized vectors. Meaning the number of joints is determined at runtime, so that various kinematic chains with different joint arrangements can be selected on the fly.

Of course, like all complex systems, KDL's features also imply some drawbacks:

1. With the explicit typing, KDL doesn't support general linear algebra. For example a Jacobian matrix has to contain 6 rows - it can't be extended (to include an additional objective) or reduced (to focus on a subspace). Or a twist can't be multiplied by a general gain matrix. So novel controllers with general mathematics are not easily prototyped. They require expanding the KDL functionality.
2. With dynamic sizing (joint selection), KDL requires dynamic memory allocation for its vectors and matrices. To remain realtime safe, KDL needs the user to pre-allocate all memory during initialization in a non-realtime thread. Linear algebra calculations which allocate temporary storage, such as matrix multiplies or inversions, are not supported.

To allow general mathematics, in particular with fixed matrix sizes (without dynamic memory allocation), ROS also supports Eigen (Eigen homepage) in the geometry stack. Eigen is a general purpose, templated library for linear algebra and general matrix operations. In fact, KDL itself uses Eigen. According to Eigen, "with fixed-size objects, dynamic memory allocation is avoided".

So we give the user two examples. The first using KDL with its solvers, strict typing, and dynamic sizing, at the cost of pre-allocation and lack of general matrix operations. The second also using KDL for the kinematic calculations, but then reverting to general, fixed-size Eigen vectors and matrices with full linear algebra support at the cost of reconfigurability. Of course, other combinations or other libraries are equally possible, though not illustrated here.

Both examples perform exactly the same task. They force the tip of the kinematic chain (the tip of the robot) to track a circular path in the Y/Z plane, of 10 cm diameter. The control is performed via a Jacobian transpose method, i.e. the controller computes a Cartesian corrective force which is then mapped to the individual joints.

Source and Header Code

Like the joint controller tutorial, we will need a source and header file. Place these in the same package you have previously created, as to leverage the remainder of the setup. In particular, you will need to create

1. src/my_cart_controller_file.cpp and

2. include/my_controller_pkg/my_cart_controller_file.h.

The code is given in the tutorials:

• Coding a realtime Cartesian controller with KDL

• Coding a realtime Cartesian controller with Eigen

Compiling

We are using the same package, but need to inform it about the additional source file and the use of the additional packages KDL and possible Eigen. Edit the CMakeLists.txt file to build the library file including both the previous joint and the new Cartesian controllers:

rosbuild_add_library(my_controller_lib src/my_controller_file.cpp
                                       src/my_cart_controller_file.cpp)

Edit the manifest.xml file to include the new dependencies:

<package>
  ...
  <depend package="eigen" />
  <depend package="kdl" />
  ...
</package>

Then build your package:

$ rosmake

Note use rosmake versus make at least once, to compile the newly added dependencies KDL and Eigen as well as the updated source code in the package. If everything went well, the library file called (lib)my_controller_lib.so in the lib folder will be updated.

Registering

As before, the we need to register the code as a plugin for the realtime process. Most pieces are still in place, but we need to edit the plugin description file controller_plugins.xml to list both controllers:

<library path="lib/libmy_controller_lib">
  <class name="MyControllerPlugin" 
         type="my_controller_ns::MyControllerClass"           
         base_class_type="pr2_controller_interface::Controller" />
  <class name="MyCartControllerPlugin" 
         type="my_controller_ns::MyCartControllerClass"           
         base_class_type="pr2_controller_interface::Controller" />
</library>

Configuring

We obviously need a different parameter set for this Cartesian controller. So we create an appropriate YAML configuration file my_cart_controller.yaml with the contents:

my_cart_controller_name:
  type: MyCartControllerPlugin
  root_name: torso_lift_link
  tip_name: r_gripper_tool_frame

Load the parameters with

$ rosparam load my_cart_controller.yaml

Running

Assuming the robot (or simulation thereof) is still running according to the previous tutorial, you can spawn the Cartesian controller via

$ rosrun pr2_controller_manager pr2_controller_manager spawn my_cart_controller_name

And, of course, to kill the controller use

$ rosrun pr2_controller_manager pr2_controller_manager kill my_cart_controller_name

Alternatively, again as before, you could also create a launch file my_cart_controller.launch:

<launch>
   <rosparam file="$(find my_controller_pkg)/my_cart_controller.yaml" command="load" />

   <node pkg="pr2_controller_manager" type="spawner" args="my_cart_controller_name" name="my_cart_controller_spawner" />
</launch>

and run it:

$ roslaunch my_controller.launch