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.

Moving the arm using the Joint Trajectory Action

Overview and prerequisites

In this tutorial we will focus on moving the arm using an action interface to the low-level Joint Trajectory Controller. Note that, at this level, we are not concerned with many aspects of arm movement, such as:

• generation of trajectories with smooth velocity and acceleration profiles
• obstacle avoidance or motion planning

Those components can be found in a higher level package called move_arm. However, if move_arm is not available, or you want to be using a lower-level interface, this tutorial shows the way.

Commanding a joint trajectory for the arm requires the following components:

• a controller that sends commands directly to the joints;

• an action interface to the controller in the form of a ROS action, that takes in a trajectory command expressed as a series of joint angles and sends the appropriate low-level commands to the controller;

• the high-level program that uses the action interface to issue the desired joint trajectories.

The first two components are available in ROS. In this tutorial we will show you how to use them by writing the third component, the high level program.

Before you begin, bring up a robot, either on the hardware or in gazebo.

The Joint Trajectory Controller

The arm trajectory controller is brought up on robot start-up. In general, a controller can be in one of three states:

• not loaded
• loaded, but not running (stopped)
• running

In general, you can use the pr2_controller_manager to check which controllers are available. After you bring up the robot, use the following command:

rosrun pr2_controller_manager pr2_controller_manager list

In the list that is printed, look for a line starting with r_arm_controller . If you find:

• r_arm_controller (running) : the controller is running; you can skip to the next section of this tutorial

• r_arm_controller (stopped) : the controller is loaded, but not running. To start it, use again the pr2_controller_manager (the arm will start to hold its current angles) :

  rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller

• no mention of r_arm_controller : the controller has not been loaded; this probably means that something went wrong during robot start-up. Abort this tutorial and investigate the cause for that.

Note that this usage of the pr2_controller_manager also applies to other controllers, such as the head trajectory controller or the gripper controller.

The Joint Trajectory Action Interface

The most convenient way of sending commands to the arm controller is through the JointTrajectoryAction, which is an implementation of a ROS action. This action takes in commands in the form of desired arm trajectories, then executes them by commanding the controller directly. For more details about actions in ROS see the actionlib package.

Note that the examples here will be given in C++, but the ROS action interface also works with Python.

The node that implements this action interface is started automatically on robot start up. To check, look at the list of topics in the r_arm_controller namespace after you bring up the robot:

rostopic list r_arm_controller

In the list of topics, you should see lines containing /r_arm_controller/joint_trajectory_action/. If they are not present, it means that either the action node was not started on robot start-up, or that it has been stopped. Abort this tutorial and investigate the cause for that.

The trajectory message

The joint trajectory action interface receives goals in the form of trajectory_msgs/JointTrajectory messages, each of which describes the trajectory that the controller will attempt to execute.

This message contains:

• a header

• a set of joint names

• a set of waypoints , each of them containing the desired position, velocity and acceleration at each joint. It is assumed that the values in the waypoints are ordered the same way as in the set of joint names contained in the message.

Note that if you only want to specify a trajectory through joint positions, you should still fill in the velocities fields with zeroes.

Set of joint names for the goal message. Both the Joint Trajectory Controller and the Joint Trajectory Action have an internal list of joints that they are operating on. This list is set when the controller and the action are started. After that, both the controller and the action expect any command to specify desired values for all of the joints they control.

By default, both the controller and the action are started up with a list of joints containing all the joints of the arm. Changing this behavior is beyond the scope of this tutorial; however, you can find out what this list of joints is by querying the ROS parameter server. You can query either the controller or the action, the lists should be similar:

rosparam get /r_arm_controller/joints

or

rosparam get /r_arm_controller/joint_trajectory_action/joints

The result should look like this:

[r_shoulder_pan_joint, r_shoulder_lift_joint, r_upper_arm_roll_joint, r_elbow_flex_joint, r_forearm_roll_joint, r_wrist_flex_joint, r_wrist_roll_joint]

The punchline is that any goal message that you send to the action must contain all of these joints (and no others) inside its joint names field. Implicitly, this also means that you must specify desired values for them in each waypoint. If these conditions are not met, the goal will be rejected as invalid. See the code example below for an example of a goal trajectory containing all required the joint names.

Time management of the trajectory. The JointTrajectory message has two fields for managing time:

• the stamp field in the header of the message

• a time_from_start value for each waypoint.

Both of these are important.

The stamp in the header determines when the execution of the trajectory starts. Once the time in the stamp field passes, the controller will begin to move towards the first waypoint in the trajectory. The controller attempts to achieve each waypoint at the time obtained by adding that waypoint's time_from_start value to the time in stamp. The time_from_start values must be monotonically non-decreasing or else the trajectory will be considered invalid.

For each point in the trajectory, put the desired joint state into positions and velocities. The controller will interpolate between these points using cubic splines. You may optionally fill in accelerations if you wish for the controller to use quintic splines. The controller will attempt to follow whatever trajectory you specify, so it is your responsibility to ensure that the desired trajectory is smooth.

Setting up your package

We will now prepare our own package for writing code to move the arm. Using the roscreate-pkg tool, you can conveniently create a package as follows:

roscreate-pkg simple_trajectory actionlib roscpp pr2_controllers_msgs

Note that we have a few dependencies for our package. One of them is roscpp as we will be writing the example in this tutorial in C++. We also depend on actionlib since we are using an action interface, and on pr2_controllers_msgs which defines the goal message we use to communicate with the action.

After creating your package, we'll need to roscd to it, since we'll be using it as our workspace.

roscd simple_trajectory

Creating the node

Put the following into src/simple_trajectory.cpp:

   1 #include <ros/ros.h>
   2 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
   3 #include <actionlib/client/simple_action_client.h>
   4 
   5 typedef actionlib::SimpleActionClient< pr2_controllers_msgs::JointTrajectoryAction > TrajClient;
   6 
   7 class RobotArm
   8 {
   9 private:
  10   // Action client for the joint trajectory action 
  11   // used to trigger the arm movement action
  12   TrajClient* traj_client_;
  13 
  14 public:
  15   //! Initialize the action client and wait for action server to come up
  16   RobotArm() 
  17   {
  18     // tell the action client that we want to spin a thread by default
  19     traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);
  20 
  21     // wait for action server to come up
  22     while(!traj_client_->waitForServer(ros::Duration(5.0))){
  23       ROS_INFO("Waiting for the joint_trajectory_action server");
  24     }
  25   }
  26 
  27   //! Clean up the action client
  28   ~RobotArm()
  29   {
  30     delete traj_client_;
  31   }
  32 
  33   //! Sends the command to start a given trajectory
  34   void startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  35   {
  36     // When to start the trajectory: 1s from now
  37     goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
  38     traj_client_->sendGoal(goal);
  39   }
  40 
  41   //! Generates a simple trajectory with two waypoints, used as an example
  42   /*! Note that this trajectory contains two waypoints, joined together
  43       as a single trajectory. Alternatively, each of these waypoints could
  44       be in its own trajectory - a trajectory can have one or more waypoints
  45       depending on the desired application.
  46   */
  47   pr2_controllers_msgs::JointTrajectoryGoal armExtensionTrajectory()
  48   {
  49     //our goal variable
  50     pr2_controllers_msgs::JointTrajectoryGoal goal;
  51 
  52     // First, the joint names, which apply to all waypoints
  53     goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  54     goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
  55     goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
  56     goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
  57     goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
  58     goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
  59     goal.trajectory.joint_names.push_back("r_wrist_roll_joint");
  60 
  61     // We will have two waypoints in this goal trajectory
  62     goal.trajectory.points.resize(2);
  63 
  64     // First trajectory point
  65     // Positions
  66     int ind = 0;
  67     goal.trajectory.points[ind].positions.resize(7);
  68     goal.trajectory.points[ind].positions[0] = 0.0;
  69     goal.trajectory.points[ind].positions[1] = 0.0;
  70     goal.trajectory.points[ind].positions[2] = 0.0;
  71     goal.trajectory.points[ind].positions[3] = 0.0;
  72     goal.trajectory.points[ind].positions[4] = 0.0;
  73     goal.trajectory.points[ind].positions[5] = 0.0;
  74     goal.trajectory.points[ind].positions[6] = 0.0;
  75     // Velocities
  76     goal.trajectory.points[ind].velocities.resize(7);
  77     for (size_t j = 0; j < 7; ++j)
  78     {
  79       goal.trajectory.points[ind].velocities[j] = 0.0;
  80     }
  81     // To be reached 1 second after starting along the trajectory
  82     goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);
  83 
  84     // Second trajectory point
  85     // Positions
  86     ind += 1;
  87     goal.trajectory.points[ind].positions.resize(7);
  88     goal.trajectory.points[ind].positions[0] = -0.3;
  89     goal.trajectory.points[ind].positions[1] = 0.2;
  90     goal.trajectory.points[ind].positions[2] = -0.1;
  91     goal.trajectory.points[ind].positions[3] = -1.2;
  92     goal.trajectory.points[ind].positions[4] = 1.5;
  93     goal.trajectory.points[ind].positions[5] = -0.3;
  94     goal.trajectory.points[ind].positions[6] = 0.5;
  95     // Velocities
  96     goal.trajectory.points[ind].velocities.resize(7);
  97     for (size_t j = 0; j < 7; ++j)
  98     {
  99       goal.trajectory.points[ind].velocities[j] = 0.0;
 100     }
 101     // To be reached 2 seconds after starting along the trajectory
 102     goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);
 103 
 104     //we are done; return the goal
 105     return goal;
 106   }
 107 
 108   //! Returns the current state of the action
 109   actionlib::SimpleClientGoalState getState()
 110   {
 111     return traj_client_->getState();
 112   }
 113  
 114 };
 115 
 116 int main(int argc, char** argv)
 117 {
 118   // Init the ROS node
 119   ros::init(argc, argv, "robot_driver");
 120 
 121   RobotArm arm;
 122   // Start the trajectory
 123   arm.startTrajectory(arm.armExtensionTrajectory());
 124   // Wait for trajectory completion
 125   while(!arm.getState().isDone() && ros::ok())
 126   {
 127     usleep(50000);
 128   }
 129   return 0;
 130 }

Building

Add the following line to the CMakeLists.txt:

rosbuild_add_executable(simple_trajectory src/simple_trajectory.cpp)

and type 'make' in the simple_trajectory directory to make your executable.

Running

Remember (from earlier in this tutorial) to make sure your robot is up (either in simulation or on hardware), the joint trajectory controller is running and the joint action node is also running.

Simply run your executable:

bin/simple_trajectory

The arm should move.