(!) 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.

Filtering joint trajectories using the trajectory_filter_server

Description: This tutorial will show you how to use the trajectory filtering service provided by the trajectory filter server.

Tutorial Level: INTERMEDIATE

Contents

  1. ROS Setup
  2. Creating the launch files for the trajectory filter nodes
    1. Trajectory filter launch file
    2. The filters yaml specification
    3. The joint limits specification
  3. Writing the client node
    1. Filling up the service request
    2. Interpreting the response
  4. Building the node
  5. Running the filter

The trajectory filters stack allows you to smooth out trajectories using a service call to a trajectory filter server. The service call uses either the FilterJointTrajectory service or the FilterJointTrajectoryWithConstraints service. In this tutorial, we will learn how to setup the trajectory node that provides this service and create a client to call it and get back a response from it.

ROS Setup

The first thing we'll need to do is create a package for all the trajectory tutorials that we will work with. To do this we'll use the handy roscreate-pkg command where we want to create the package directory with a dependency on the motion_planning_msgs packages as shown below: 

roscreate-pkg trajectory_tutorials motion_planning_msgs trajectory_filter_server pr2_description

You ordinarily do not need a dependency on the trajectory_filter_server package to use the trajectory filter server node. However, we will add the dependency here since we are going to configure and launch the node from the directory we just created.

After this is done we'll need to roscd to the package we created, since we'll be using it as our workspace 

roscd trajectory_tutorials

Also, make sure to set an environment variable called ROBOT to sim. E.g., in a bash shell, 

 export ROBOT=sim

(Note: you need to do this only if you are running this tutorial on a simulator).

Creating the launch files for the trajectory filter nodes

To launch trajectory filter node, we need to configure three things:

  1. The launch file for the node itself.
  2. The yaml specification file for the set of filters we need
  3. The yaml specification file for the joint limits we want to specify

Trajectory filter launch file

Here's the launch file for the trajectory filter node. Its a simple launch file that launches the node itself and two yaml files that we will now learn how to configure. Save this text to a file called trajectory_filter.launch. 

<launch>
    <!-- send pr2 urdf to param server -->
    <param name="robot_description" command="$(find xacro)/xacro.py '$(find pr2_description)/robots/pr2.urdf.xacro'" />

  <node pkg="trajectory_filter_server" name="trajectory_filter" type="trajectory_filter_server" output="screen">
   <rosparam command="load" file="$(find trajectory_tutorials)/joint_limits.yaml"/>
   <rosparam command="load" file="$(find trajectory_tutorials)/filters.yaml"/>
  </node>
</launch>

The filters yaml specification

The trajectory filters are templated. Most filters will be able to take 3 different messages as input:

The trajectory filter server sets up a chain of filters through which it will pass the input message. The set of filters are specified in the filters.yaml file. Here's an example of a filters.yaml file: 

service_type: FilterJointTrajectory
filter_chain:
  - 
    name: numerical_differentiation
    type: NumericalDifferentiationSplineSmootherFilterJointTrajectory
  -
    name: linear_spline_velocity_scaler
    type: LinearSplineVelocityScalerFilterJointTrajectory

The first field (service_type) can take two values: FilterJointTrajectory or FilterJointTrajectoryWithConstraints. When service_type: FilterJointTrajectory, the trajectory filter server is internally setup to listen for a service call of type FilterJointTrajectoryService. When service_type: FilterJointTrajectoryWithConstraints, the trajectory filter server is internally setup to listen for a service call of type FilterJointTrajectoryServiceWithConstraints.

This yaml file specifies two trajectory filters. Each filter has a filter type and a name that differentiates it from filters of the same type. The input trajectory will be passed through the first filter which is of type NumericalDifferentiationSplineSmoother and then through the second filter which is of type LinearSplineVelocityScaler. The first filter uses numerical differentiation to add velocities to a trajectory with only positions filled up while the second filter linearly scales the timing between consecutive waypoints of the trajectory to make sure velocities are within the joint limits specified in the joint limits file.

If you wanted two filters of the same type, make sure you give them two different names.

How do you figure out which filters are available? Well, we are working on a tool that will let you do that easily.

The joint limits specification

Here's an example joint_limits.yaml file showing the joint limits specification for two joints. The fields in this yaml file correspond to the fields in the JointLimits message. 

joint_limits:
  r_shoulder_pan_joint:
    has_position_limits: true
    min_position: -2.2853981634
    max_position: 0.714601836603
    has_velocity_limits: true
    max_velocity: 0.8
    has_acceleration_limits: true
    max_acceleration: 0.5
  r_shoulder_lift_joint:
    has_position_limits: true
    min_position: -0.5236
    max_position: 0.3963
    has_velocity_limits: true
    max_velocity: 0.82
    has_acceleration_limits: true
    max_acceleration: 0.5

Note that in addition to specifying joint limits, you also need to set the flags for whether the particular limits exist.

Writing the client node

We will now write a client node that creates a dummy trajectory and calls the FilterJointTrajectory service to filter it. Fire up a text editor and paste the following into a file called src/filter_trajectory.cpp. Don't worry if there are things you don't understand, we'll walk through the details of this file line-by-line shortly. 

   1 #include <ros/ros.h>
   2 #include <motion_planning_msgs/FilterJointTrajectory.h>
   3 
   4 int main(int argc, char **argv){
   5   ros::init (argc, argv, "filter_joint_trajectory");
   6   ros::NodeHandle rh;
   7   ros::service::waitForService("trajectory_filter/filter_trajectory");
   8   motion_planning_msgs::FilterJointTrajectory::Request req;
   9   motion_planning_msgs::FilterJointTrajectory::Response res;
  10   ros::ServiceClient filter_trajectory_client_ = rh.serviceClient<motion_planning_msgs::FilterJointTrajectory>("trajectory_filter/filter_trajectory");
  11 
  12   req.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  13   req.trajectory.joint_names.push_back("r_shoulder_lift_joint");
  14   req.trajectory.points.resize(3);
  15 
  16   for(unsigned int i=0; i < 3; i++)
  17   {    
  18     req.trajectory.points[i].positions.resize(2);
  19   }
  20   
  21   req.trajectory.points[1].positions[0] = 0.5;
  22   req.trajectory.points[2].positions[0] = -1.5;
  23 
  24   req.trajectory.points[1].positions[1] = 0.2;
  25   req.trajectory.points[2].positions[1] = -0.5;
  26 
  27   req.allowed_time = ros::Duration(1.0);
  28 
  29   if(filter_trajectory_client_.call(req,res))
  30   {
  31     if(res.error_code.val == res.error_code.SUCCESS)
  32     {
  33       ROS_INFO("Requested trajectory was filtered");
  34       for(unsigned int i=0; i < res.trajectory.points.size(); i++)
  35       {
  36         ROS_INFO_STREAM(res.trajectory.points[i].positions[0] << "," << res.trajectory.points[i].velocities[0] << "," << res.trajectory.points[i].positions[1] << "," << res.trajectory.points[i].velocities[1] << "," << res.trajectory.points[i].time_from_start.toSec());
  37       }
  38     }
  39     else
  40       ROS_INFO("Requested trajectory was not filtered. Error code: %d",res.error_code.val);
  41   }
  42   else
  43   {
  44     ROS_ERROR("Service call to filter trajectory failed %s",filter_trajectory_client_.getService().c_str());
  45   }
  46   ros::shutdown();
  47 }

Filling up the service request

To call the service request we need to fill it up first with our desired input trajectory. We will fill the joint trajectory for the two joints specified in our joints.yaml file, the r_shoulder_pan_joint and the r_shoulder_lift_joint. 

  12   req.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  13   req.trajectory.joint_names.push_back("r_shoulder_lift_joint");
  14   req.trajectory.points.resize(3);
  15 
  16   for(unsigned int i=0; i < 3; i++)
  17   {    
  18     req.trajectory.points[i].positions.resize(2);
  19   }
  20   
  21   req.trajectory.points[1].positions[0] = 0.5;
  22   req.trajectory.points[2].positions[0] = -1.5;
  23 
  24   req.trajectory.points[1].positions[1] = 0.2;
  25   req.trajectory.points[2].positions[1] = -0.5;

We also specify an allowed time for the filtering. 

  27   req.allowed_time = ros::Duration(1.0);

Interpreting the response

After calling the service, we need to check the error code on the service response to determine whether the input trajectory was filtered successfully. 

  31     if(res.error_code.val == res.error_code.SUCCESS)

Building the node

Now that we have a package and a source file, we'll want to build and then try things out. The first step will be to add our src/filter_trajectory.cpp file to our CMakeLists.txt file to get it to build. Open up CMakeLists.txt in your editor of choice and add the following line to the bottom of the file. 

rosbuild_add_executable(filter_trajectory src/filter_trajectory.cpp)

Once this is done, we can build our executable by typing make. 

make

Running the filter

First launch the trajectory_filter.launch file from above. Make sure the location of the two yaml files you have created in this tutorial matches the location specified in the launch file. 

roslaunch trajectory_filter.launch

Now, run the executable you created earlier. 

./bin/filter_trajectory

If it succeeds, you should see the following output 

[ INFO] 1266027635.355954000: Requested trajectory was filtered
[ INFO] 1266027635.356097000: 0,0.8,0,0.32,0
[ INFO] 1266027635.356166000: 0.5,0.8,0.2,0.32,0.625
[ INFO] 1266027635.356205000: -1.5,-0.8,-0.5,-0.28,3.125

Wiki: trajectory_filters/Tutorials/Tutorial 1 (last edited 2013-05-03 12:14:09 by DenisStogl)

Except where otherwise noted, the ROS wiki is licensed under the
Creative Commons Attribution 3.0