| Note: This tutorial assumes that you are in the middle of the tutorial: Implementing a realtime Cartesian 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. |
Coding a realtime Cartesian controller with KDL
Description: This tutorial gives example code for a realtime Cartesian controller using KDLTutorial Level: ADVANCED
Next Tutorial: See also the code example using Eigen
Introduction
The following gives example code for a realtime Cartesian controller, to be run from the previous tutorial.
Like the joint controller, we create a class which inherits from the controller::Controller class in the pr2_controller_interface package. As before, it overloads the init, starting, update, and stopping methods to provide the desired realtime functionality.
To access the joints, we use the Chain object, which builds a chain of joints from the specified root to tip frames and then reads the joint position or velocity vector as well as sets the joint torque vector.
To perform the kinematic calculations, we use the KDL library, which means we utilize "solvers" (functions) to calculate the forward kinematics and Jacobian matrix. The main point to remember is that KDL requires pre-allocation of all vectors and matrices.
The Header File
The following gives the header file, which should be created as include/my_controller_pkg/my_cart_controller_file.h:
1 #include <pr2_controller_interface/controller.h>
2 #include <pr2_mechanism_model/chain.h>
3 #include <pr2_mechanism_model/robot.h>
4
5 #include <boost/scoped_ptr.hpp>
6 #include <kdl/chain.hpp>
7 #include <kdl/chainjnttojacsolver.hpp>
8 #include <kdl/chainfksolverpos_recursive.hpp>
9 #include <kdl/frames.hpp>
10 #include <kdl/jacobian.hpp>
11 #include <kdl/jntarray.hpp>
12
13
14 namespace my_controller_ns{
15
16 class MyCartControllerClass: public pr2_controller_interface::Controller
17 {
18 private:
19 // The current robot state (to get the time stamp)
20 pr2_mechanism_model::RobotState* robot_state_;
21
22 // The chain of links and joints
23 pr2_mechanism_model::Chain chain_;
24 KDL::Chain kdl_chain_;
25
26 // KDL Solvers performing the actual computations
27 boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
28 boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;
29
30 // The variables (which need to be pre-allocated).
31 KDL::JntArray q_; // Joint positions
32 KDL::JntArray q0_; // Joint initial positions
33 KDL::JntArrayVel qdot_; // Joint velocities
34 KDL::JntArray tau_; // Joint torques
35
36 KDL::Frame x_; // Tip pose
37 KDL::Frame xd_; // Tip desired pose
38 KDL::Frame x0_; // Tip initial pose
39
40 KDL::Twist xerr_; // Cart error
41 KDL::Twist xdot_; // Cart velocity
42 KDL::Wrench F_; // Cart effort
43 KDL::Jacobian J_; // Jacobian
44
45 // Note the gains are incorrectly typed as a twist,
46 // as there is no appropriate type!
47 KDL::Twist Kp_; // Proportional gains
48 KDL::Twist Kd_; // Derivative gains
49
50 // The trajectory variables
51 double circle_phase_; // Phase along the circle
52 ros::Time last_time_; // Time of the last servo cycle
53
54 public:
55 bool init(pr2_mechanism_model::RobotState *robot,
56 ros::NodeHandle &n);
57 void starting();
58 void update();
59 void stopping();
60 };
61 }
We see the header defines the class and includes storage for the robot state pointer (to allow access to the sample time) as well as the chain of joints.
After that it defines pointers to two KDL solvers (functions): one for computing the forward kinematics (tip position), the other for computing the Jacobian matrix.
We also see all controller variables are pre-defined so that they can be pre-allocated. Note in particular that the Kp/Kd (position and velocity) gains are defined as KDL::Twist 6x1 vectors. While they are strictly not a twist vector, no other appropriate type exists and the size is correct.
Finally, the header file defines two variables for the state of the trajectory and overloads the init(), starting(), update(), stopping() methods.
The Source Code
The following gives the source code file, which should be created as src/my_cart_controller_file.cpp:
1 #include "my_controller_pkg/my_cart_controller_file.h"
2 #include <pluginlib/class_list_macros.h>
3
4 using namespace my_controller_ns;
5
6
7 /// Controller initialization in non-realtime
8 bool MyCartControllerClass::init(pr2_mechanism_model::RobotState *robot,
9 ros::NodeHandle &n)
10 {
11 // Get the root and tip link names 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 // Construct a chain from the root to the tip and prepare the kinematics.
27 // Note the joints must be calibrated.
28 if (!chain_.init(robot, root_name, tip_name))
29 {
30 ROS_ERROR("MyCartController could not use the chain from '%s' to '%s'",
31 root_name.c_str(), tip_name.c_str());
32 return false;
33 }
34
35 // Store the robot handle for later use (to get time).
36 robot_state_ = robot;
37
38 // Construct the kdl solvers in non-realtime.
39 chain_.toKDL(kdl_chain_);
40 jnt_to_pose_solver_.reset(new KDL::ChainFkSolverPos_recursive(kdl_chain_));
41 jnt_to_jac_solver_.reset(new KDL::ChainJntToJacSolver(kdl_chain_));
42
43 // Resize (pre-allocate) the variables in non-realtime.
44 q_.resize(kdl_chain_.getNrOfJoints());
45 q0_.resize(kdl_chain_.getNrOfJoints());
46 qdot_.resize(kdl_chain_.getNrOfJoints());
47 tau_.resize(kdl_chain_.getNrOfJoints());
48 J_.resize(kdl_chain_.getNrOfJoints());
49
50 // Pick the gains.
51 Kp_.vel(0) = 100.0; Kd_.vel(0) = 1.0; // Translation x
52 Kp_.vel(1) = 100.0; Kd_.vel(1) = 1.0; // Translation y
53 Kp_.vel(2) = 100.0; Kd_.vel(2) = 1.0; // Translation z
54 Kp_.rot(0) = 100.0; Kd_.rot(0) = 1.0; // Rotation x
55 Kp_.rot(1) = 100.0; Kd_.rot(1) = 1.0; // Rotation y
56 Kp_.rot(2) = 100.0; Kd_.rot(2) = 1.0; // Rotation z
57
58 return true;
59 }
60
61 /// Controller startup in realtime
62 void MyCartControllerClass::starting()
63 {
64 // Get the current joint values to compute the initial tip location.
65 chain_.getPositions(q0_);
66 jnt_to_pose_solver_->JntToCart(q0_, x0_);
67
68 // Initialize the phase of the circle as zero.
69 circle_phase_ = 0.0;
70
71 // Also reset the time-of-last-servo-cycle.
72 last_time_ = robot_state_->getTime();
73 }
74
75
76 /// Controller update loop in realtime
77 void MyCartControllerClass::update()
78 {
79 double dt; // Servo loop time step
80
81 // Calculate the dt between servo cycles.
82 dt = (robot_state_->getTime() - last_time_).toSec();
83 last_time_ = robot_state_->getTime();
84
85 // Get the current joint positions and velocities.
86 chain_.getPositions(q_);
87 chain_.getVelocities(qdot_);
88
89 // Compute the forward kinematics and Jacobian (at this location).
90 jnt_to_pose_solver_->JntToCart(q_, x_);
91 jnt_to_jac_solver_->JntToJac(q_, J_);
92
93 for (unsigned int i = 0 ; i < 6 ; i++)
94 {
95 xdot_(i) = 0;
96 for (unsigned int j = 0 ; j < kdl_chain_.getNrOfJoints() ; j++)
97 xdot_(i) += J_(i,j) * qdot_.qdot(j);
98 }
99
100 // Follow a circle of 10cm at 3 rad/sec.
101 circle_phase_ += 3.0 * dt;
102 KDL::Vector circle(0,0,0);
103 circle(2) = 0.1 * sin(circle_phase_);
104 circle(1) = 0.1 * (cos(circle_phase_) - 1);
105
106 xd_ = x0_;
107 xd_.p += circle;
108
109 // Calculate a Cartesian restoring force.
110 xerr_.vel = x_.p - xd_.p;
111 xerr_.rot = 0.5 * (xd_.M.UnitX() * x_.M.UnitX() +
112 xd_.M.UnitY() * x_.M.UnitY() +
113 xd_.M.UnitZ() * x_.M.UnitZ());
114
115 for (unsigned int i = 0 ; i < 6 ; i++)
116 F_(i) = - Kp_(i) * xerr_(i) - Kd_(i) * xdot_(i);
117
118 // Convert the force into a set of joint torques.
119 for (unsigned int i = 0 ; i < kdl_chain_.getNrOfJoints() ; i++)
120 {
121 tau_(i) = 0;
122 for (unsigned int j = 0 ; j < 6 ; j++)
123 tau_(i) += J_(j,i) * F_(j);
124 }
125
126 // And finally send these torques out.
127 chain_.setEfforts(tau_);
128 }
129
130
131 /// Controller stopping in realtime
132 void MyCartControllerClass::stopping()
133 {}
134
135
136
137 /// Register controller to pluginlib
138 PLUGINLIB_REGISTER_CLASS(MyCartControllerPlugin,
139 my_controller_ns::MyCartControllerClass,
140 pr2_controller_interface::Controller)
The init() method first constructs the chain of joints, using the root and tip link names. It also stores the robot state pointer, simply to be able to access the time of every servo cycle in the update loop. Then it configures KDL. This means instantiating two solvers and storing pointers to them. Instantiation of the solvers requires knowledge of the kinematic chain, in particular so that the objects can pre-allocate sufficient memory. Finally, the init() methods sets the Cartesian gains, in translation and orientation.
The starting() method resets trajectory variables. That is, it remembers the initial Cartesian position. To calculate the Cartesian position, it first reads the joint positions from the chain object. Then it uses KDL to calculate the matching tip position using the forward kinematics.
The update() method similarly starts by calculating the current tip information. It reads both the joint positions and velocities via the chain object. Then calculates the tip position and Jacobian via the KDL solvers. The tip velocity is simply J*qdot. Notice that the KDL types do not allow this multiplication, so that an explicit loop is required.
The middle portion of the update() method defines the desired trajectory. It simply integrates the current phase in the circle and sets the desired position appropriately shifted relative to the initial position. Note xd_.p means only the position is shifted and the orientation is left untouched.
To calculate a corrective force update() first computes a error vector, then multiples with proportional gains and adds damping. This force is converted to joint torques using a Jacobian transpose and sent to the actuators via the chain object. Again explicit loops are required to perform the multiplications and transpose operations.
Compiling and Running
To compile and run the controller, go back to the previous tutorial.
