[Documentation] [TitleIndex] [WordIndex

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 space

Tutorial Level: ADVANCED

Next Tutorial: Running a realtime Cartesian controller

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

   3 #include <pr2_mechanism_model/chain.h>
   4 

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.

   4 #include <boost/scoped_ptr.hpp>
   5 #include <kdl/chainjnttojacsolver.hpp>
   6 #include <kdl/chainfksolverpos_recursive.hpp>
   7 

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.

  16   KDL::JntArray jnt_pos_, jnt_effort_;
  17   KDL::Jacobian jacobian_;
  18   KDL::Frame reference_pose_;

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.

  20   boost::scoped_ptr<KDL::ChainFkSolverPos> jnt_to_pose_solver_;
  21   boost::scoped_ptr<KDL::ChainJntToJacSolver> jnt_to_jac_solver_;

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.

  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   }

Here we construct the chain object from the robot model.

  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));

Here we extract a kdl chain from the chain object, and use it to construct the solvers.

  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());

Here we resize the arrays, so we do the memory allocation in non-realtime.

  62   // computes Cartesian pose in realtime
  63 

Here we get the joint state (position) form the chain object.

  65   jnt_to_pose_solver_->JntToCart(jnt_pos_, current_pose);
  66 
  67   // get the pose error
  68 

Here we actually compute the forward position kinematics, the forward velocity kinematics, and the robot Jacobian. All these solvers are realtime-safe.

  78   // jnt_effort = Jac^transpose * cart_wrench
  79 

Here we compute the robot Jacobian in realtime

  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 

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.


2019-12-07 12:57