[Documentation] [TitleIndex] [WordIndex

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 KDL

Tutorial 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.


2019-10-19 13:02