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 Stiffness Controller (C++)

Downloading the Controller

If you already have already checked out the mit-ros-pkg, you can skip this step.

Download the ee_cart_imped stack, which contains the ee_cart_imped_control, ee_cart_imped_action, ee_cart_imped_launch, and ee_cart_imped_msgs packages, using these instructions.

You now have all the files needed for running the controller and the action server. We will discuss how to compile and run them in the next tutorial.

The Code

First, create a package for this tutorial. Open a new shell, navigate to a directory on your ROS_PACKAGE_PATH in which you wish to do the tutorial and type

roscreate-pkg ee_cart_imped_tutorial actionlib roscpp ee_cart_imped_action ee_cart_imped_msgs pr2_controllers_msgs

Within that package, create the src/stiffness_control.cpp file and paste the following inside it:

   1 #include <ee_cart_imped_action/ee_cart_imped_arm.hh>
   2 #include <ee_cart_imped_msgs/EECartImpedGoal.h>
   3 
   4 int main(int argc, char **argv) {
   5   ros::init(argc, argv, "stiffness_control_test");
   6 
   7   /**
   8    *The EECartImpedArm class is a wrapper for an action client to the
   9    *ee_cart_imped_action server.  The argument "r_arm_cart_imped_controller" 
  10    *tells the client that it is a client for the server that controls the 
  11    *right arm
  12    */
  13   EECartImpedArm arm("r_arm_cart_imped_controller");
  14 
  15   /**
  16    *This variable will hold the trajectory as we create it.
  17    */
  18   ee_cart_imped_msgs::EECartImpedGoal traj;
  19 
  20   /**
  21    *addTrajectoryPoint is a static function in the EECartImpedArm class that 
  22    *adds a trajectory point to the end of the first argument.  It simply 
  23    *assigns each value in the goal structure for us to prevent having to 
  24    *write it all out.
  25    *
  26    *This is a point in the center of the robot's body.  This simply moves the 
  27    *arm to that point with maximum stiffness.  
  28    */
  29   EECartImpedArm::addTrajectoryPoint(traj, 0.5, 0, 0, 0, 0, 0, 1,
  30                                      1000, 1000, 1000, 30, 30, 30,
  31                                      false, false, false, false, false,
  32                                      false, 4);
  33   /**
  34    *This point is farther in front of the robot, but it is only allowed to 
  35    *use a very small stiffness in the x direction
  36    */
  37   EECartImpedArm::addTrajectoryPoint(traj, 0.75, 0, 0, 0, 0, 0, 1,
  38                                      50, 1000, 1000, 30, 30, 30,
  39                                      false, false, false, false, false,
  40                                      false, 6);
  41   /**
  42    *This is the line that actually sends the trajectory to the action server
  43    *and starts the arm moving.  The server will block until the arm completes 
  44    *the trajectory or it is aborted.
  45    */
  46   arm.startTrajectory(traj);
  47 }

Client-Server-Controller Model

Before we examine the code in depth, let's go through the basic outline of the ROS message passing for our controller. The diagram below shows the structure

At the bottom level, we have the force/compliance realtime controller from the ee_cart_imped_control package. However, the controller should never be accessed directly at this level because there is no monitoring. For example, if the controller is shut off, there is no way of knowing that. Therefore, all interaction with the controller should be done through the ee_cart_imped_action, which monitors the controller and the trajectory and can abort the trajectory if the controller is no longer responding or the trajectory violates user-defined constraints. The action itself is an action server.

To communicate with an action server, an action client is used. You may write your own client to interact with the ee_cart_imped_action, but we have also provided one in the EECartImpedArm class. This class is a wrapper around the simple action client and should be sufficient for almost all uses of the controller.

Examining the Code

Now, let's break the code down.

   1 #include <ee_cart_imped_action/ee_cart_imped_arm.hh>
   2 #include <ee_cart_imped_msgs/EECartImpedGoal.h>
   3 

This includes the file that contains the EECartImpedArm class and the file that contains the EECartImpedGoal message. The EECartImpedArm class does all the work of interacting with the action server so you do not need to include those files.

   7   /**
   8    *The EECartImpedArm class is a wrapper for an action client to the
   9    *ee_cart_imped_action server.  The argument "r_arm_cart_imped_controller" 
  10    *tells the client that it is a client for the server that controls the 
  11    *right arm
  12    */
  13   EECartImpedArm arm("r_arm_cart_imped_controller");

This is the instantiation of the EECartImpedArm. When instantiating the class, you must pass it the namespace of the action server. This effectively tells the client which arm we want to control. Here, we are controlling the right arm. If we wanted to control the left arm, we would pass in the argument "l_arm_cart_imped_controller".

  15   /**
  16    *This variable will hold the trajectory as we create it.
  17    */
  18   ee_cart_imped_msgs::EECartImpedGoal traj;

The action client sends a ee_cart_imped_msgs/EECartImpedActionGoal to the server. This instantiates the goal we will eventually send, but first we must specify the trajectory.

  20   /**
  21    *addTrajectoryPoint is a static function in the EECartImpedArm class that 
  22    *adds a trajectory point to the end of the first argument.  It simply 
  23    *assigns each value in the goal structure for us to prevent having to 
  24    *write it all out.
  25    *
  26    *This is a point in the center of the robot's body.  This simply moves the 
  27    *arm to that point with maximum stiffness.  
  28    */
  29   EECartImpedArm::addTrajectoryPoint(traj, 0.5, 0, 0, 0, 0, 0, 1,
  30                                      1000, 1000, 1000, 30, 30, 30,
  31                                      false, false, false, false, false,
  32                                      false, 4, "/torso_lift_link");
  33   /**
  34    *This point is farther in front of the robot, but it is only allowed to 
  35    *use a very small stiffness in the x direction
  36    */
  37   EECartImpedArm::addTrajectoryPoint(traj, 0.75, 0, 0, 0, 0, 0, 1,
  38                                      50, 1000, 1000, 30, 30, 30,
  39                                      false, false, false, false, false,
  40                                      false, 6, "/torso_lift_link");

Here, we are filling in a two point trajectory. The EECartImpedArm::addTrajectoryPoint function is a simple function that takes in all of the numbers necessary for a trajectory point, creates the point and adds it as the last point on the trajectory that is passed in as the first argument. Specifically, addTrajectoryPoint(goal, x, y, z, ox, oy, oz, ow, fx, fy, fz, tx, ty, tz, isfx, isfy, isfz, istx, isty, istz, time) adds a point to goal with position (x, y, z) and orientation as the quaternion (ox, oy, oz, ow). The force or stiffness in the x, y, and z directions are given by fx, fy, and fz respectively. The torque or rotational stiffness around the x, y, and z axes are given by tx, ty, and tz respectively. The booleans isfx, isfy, and isfz are true if the arm should exert a force in the x, y, or z directions respectively and false if it should maintain a stiffness. Similarly, the booleans istx, isty, istz are true if the arm should exert a torque around the x, y, or z axes respectively and false if it should maintain a rotational stiffness. The time is the time from the starting time of the trajectory that this point should be reached. For the two points shown here, the PR2 will first move its gripper to approximately the center of its body using maximum stiffness. This move will take 4 seconds. It will then extend the gripper forward, but using only a very small stiffness in the x direction. This move will also take 2 seconds as it is scheduled to complete 6 seconds from the start of the whole trajectory.

WARNING: To prevent overdriving the controller, rotational stiffness should never be set above 75. Translational stiffness should never be set above 1000.

  41   /**
  42    *This is the line that actually sends the trajectory to the action server
  43    *and starts the arm moving.  The server will block until the arm completes 
  44    *the trajectory or it is aborted.
  45    */
  46   arm.startTrajectory(traj);

Once the trajectory we have finished creating the trajectory, we send it to the server and start the execution. This line will block until the trajectory completes or is aborted.

Running the Code

Now that we have written a stiffness control example, we need to run the code.