|Note: This tutorial assumes that you have completed the previous tutorials: Running a realtime joint controller.|
|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.|
Implementing a realtime Cartesian controllerDescription: 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
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.
defines and understands (open) kinematic chains, so knowledge of the kinematic parameters (lengths, offsets, etc.) is sufficient to perform all calculations.
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.
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
- 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:
- 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.
- 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
The code is given in the tutorials:
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:
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.
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>
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
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