| Note: This tutorial assumes that you have completed the previous tutorials: Writing 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. |
Writing a realtime Cartesian controller
Description: This tutorial teaches you how to control a robot in Cartesian spaceTutorial Level: ADVANCED
Next Tutorial: Running a realtime Cartesian controller
Contents
Introduction
This tutorial teaches you how to create a realtime controller that works in Cartesian space. So if you want to know more about things like the kinematic/dynamic robot model, forward/inverse kinematics or even dynamics, Jacobians, etc, this tutorial is what you need. In order to understand this tutorial, you should have run through the writing a realtime joint controller tutorial.
Writing the code
First, let's again create a package with the correct dependencies, and build the dependencies:
$ roscreate-pkg my_controller_cart_pkg pr2_controller_interface pr2_mechanism_model pluginlib $ roscd my_controller_cart_pkg $ rosmake
Now create a header file called include/my_controller_cart_pkg/my_controller_cart.h:
1 #include <pr2_controller_interface/controller.h>
2 #include <pr2_mechanism_model/joint.h>
3 #include <pr2_mechanism_model/chain.h>
4 #include <boost/scoped_ptr.hpp>
5 #include <kdl/chainjnttojacsolver.hpp>
6 #include <kdl/chainfksolverpos_recursive.hpp>
7
8 namespace my_controller{
9
10 class MyControllerCart: public pr2_controller_interface::Controller
11 {
12 private:
13 pr2_mechanism_model::JointState* joint_state_;
14 pr2_mechanism_model::Chain chain_;
15
16 KDL::JntArray jnt_pos_, jnt_effort_;
17 KDL::Jacobian jacobian_;
18 KDL::Frame reference_pose_;
19
20 boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
21 boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
22
23 public:
24 bool init(pr2_mechanism_model::RobotState *robot,
25 ros::NodeHandle &n);
26 void starting();
27 void update();
28 void stopping();
29 };
30 }
Now, let's take a look at the cpp file src/my_controller_cart.cpp:
1 #include "my_controller_cart_pkg/my_controller_cart.h"
2 #include <pluginlib/class_list_macros.h>
3
4 using namespace my_controller;
5
6
7 /// Controller initialization in non-realtime
8 bool MyControllerCart::init(pr2_mechanism_model::RobotState *robot,
9 ros::NodeHandle &n)
10 {
11 // gets root and tip name from parameter server
12 std::string root_name, tip_name;
13 if (!n.getParam("root_name", root_name))
14 {
15 ROS_ERROR("No root name given in namespace: %s)",
16 n.getNamespace().c_str());
17 return false;
18 }
19 if (!n.getParam("tip_name", tip_name))
20 {
21 ROS_ERROR("No tip name given in namespace: %s)",
22 n.getNamespace().c_str());
23 return false;
24 }
25
26 // constructs a chain from root to tip
27 if (!chain_.init(robot, root_name, tip_name))
28 {
29 ROS_ERROR("MyControllerCart could not find a chain from '%s' to '%s'",
30 root_name.c_str(), tip_name.c_str());
31 return false;
32 }
33
34 // constructs the kdl solvers in non-realtime
35 KDL::Chain kdl_chain;
36 chain_.toKDL(kdl_chain);
37 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain));
38 jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain));
39
40 // resizes the joint state vectors in non-realtime
41 jnt_pos_.resize(kdl_chain.getNrOfJoints());
42 jnt_effort_.resize(kdl_chain.getNrOfJoints());
43 jacobian_.resize(kdl_chain.getNrOfJoints());
44
45 // sets the desired pose
46 reference_pose_ = KDL::Frame(KDL::Rotation::RPY(0,0,0),
47 KDL::Vector(0.7, -0.2, 0));
48
49 return true;
50 }
51
52
53 /// Controller startup in realtime
54 void MyControllerCart::starting()
55 {}
56
57 /// Controller update loop in realtime
58 void MyControllerCart::update()
59 {
60 // get the joint positions of the chain
61 chain_.getPositions(jnt_pos_);
62
63 // computes Cartesian pose in realtime
64 KDL::Frame current_pose;
65 jnt_to_pose_solver_->JntToCart(jnt_pos_, current_pose);
66
67 // get the pose error
68 KDL::Frame offset;
69 KDL::Twist error;
70 offset = KDL::Frame(KDL::Rotation::RPY(0,0,0),
71 KDL::Vector(0,
72 0.2*sin(0.5*ros::Time::now().toSec()),
73 0.2*cos(0.5*ros::Time::now().toSec())));
74 error = KDL::diff(current_pose, reference_pose_*offset);
75
76 // compute Jacobian in realtime
77 jnt_to_jac_solver_->JntToJac(jnt_pos_, jacobian_);
78
79 // jnt_effort = Jac^transpose * cart_wrench
80 for (unsigned int i = 0; i < jnt_pos_.rows(); i++)
81 {
82 jnt_effort_(i) = 0;
83 for (unsigned int j=0; j<6; j++)
84 jnt_effort_(i) += (jacobian_(j,i) * 30.0 * error(j));
85 }
86
87 // send joint efforts to robot
88 chain_.setEfforts(jnt_effort_);
89 }
90
91
92 /// Controller stopping in realtime
93 void MyControllerCart::stopping()
94 {}
95
96
97 /// Register controller to pluginlib
98 PLUGINLIB_REGISTER_CLASS(MyControllerCart,
99 my_controller::MyControllerCart,
100 pr2_controller_interface::Controller)
The code explained
.h
Here we include the chain helper class. This class makes it easy to construct a kinematic chain from the urdf robot model, and gives direct access to joint positions, velocities and efforts.
Here we include the kdl solvers we are using in this example. Kdl provides many more solvers, so take a look at the kdl documentation to find the solver you need.
Here we declare the array that will store the joint positions, and the robot Jacobian. These variables need to be class members, because we need to resize them to the chain length once, in non-realtime, and then use them in the realtime loop.
Here we declare the kdl solvers to compute forward position kinematics, and Jacobian computation. We use boost scoped pointers to avoid problems with memory leaks etc.
.cpp
11 // gets root and tip name from parameter server
12 std::string root_name, tip_name;
13 if (!n.getParam("root_name", root_name))
14 {
15 ROS_ERROR("No root name given in namespace: %s)",
16 n.getNamespace().c_str());
17 return false;
18 }
19 if (!n.getParam("tip_name", tip_name))
20 {
21 ROS_ERROR("No tip name given in namespace: %s)",
22 n.getNamespace().c_str());
23 return false;
24 }
Here we get the root and tip name of our kinematic chain from the parameter server.
Here we construct the chain object from the robot model.
Here we extract a kdl chain from the chain object, and use it to construct the solvers.
Here we resize the arrays, so we do the memory allocation in non-realtime.
Here we get the joint state (position) form the chain object.
Here we actually compute the forward position kinematics, the forward velocity kinematics, and the robot Jacobian. All these solvers are realtime-safe.
Here we compute the robot Jacobian in realtime
Here we multiply the pose error with the Jacobian transpose, to get desired joint efforts.
89 }
Here we send the desired joint efforts to the robot to get executed.
Compiling the code
Now that we created the code, lets compile it first. Open the CMakeLists.txt file, and add the following line on the bottom:
rosbuild_add_library(my_controllers src/my_controller_cart.cpp)
and, try to build your package:
$ make
If everything went well, you should have a library file called my_controllers.so in your lib folder.
Register your controller
Controllers are loaded from within pr2_controller_manager using pluginlib. To make the controllers visible from pr2_controller_manager, the package containing the controller must export it as a loadable class.
To export the controller, you must depend on the pr2_controller_interface package and reference the plugin description file in the exports section. Here are some example snippets from a manifest.xml doing just that:
<package>
...
<depend package="pr2_controller_interface" />
<depend package="pluginlib" />
...
<export>
<pr2_controller_interface plugin="${prefix}/controller_plugins.xml" />
</export>
...
</package>You should then create the controller_plugins.xml file. The format is described in the pluginlib documentation. For our controller we need the following description in controller_plugins.xml:
<library path="lib/libmy_controllers">
<class name="MyControllerCart"
type="my_controller::MyControllerCart"
base_class_type="pr2_controller_interface::Controller" />
</library>Now, let's ensure that the controller is correctly configured as a plugin and is visible to Mechanism Control. Check that the plugin description file is visible to rospack:
$ rospack plugins --attrib=plugin pr2_controller_interface
Your controller_plugins.xml file should be in this list. If it's not, then you probably did not add pr2_controller_interface as a dependency of your package.
Now you're ready for the next tutorial, that will teach you how to run your controller.
There's still no good way of checking that plugins are built without using nm. https://code.ros.org/trac/ros-pkg/ticket/2822