[Documentation] [TitleIndex] [WordIndex

Note: This tutorial only works using Diamondback. For a roughly equivalent tutorial in Electric please see Checking Trajectory Validity. This tutorial assumes that you have completed the previous tutorials: Checking collisions for a given robot state.
(!) 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.

Checking collisions for a joint trajectory

Description: This tutorial will show you how to check whether an input joint trajectory is in collision, violates joint limits or satisfies constraints.

Tutorial Level: ADVANCED

Next Tutorial: ''Safe'' arm trajectory control

In this tutorial, you will learn how to use the environment_server node to check collisions, joint limits and constraints for a given joint trajectory.

ROS Setup

The ROS setup procedure was explained in detail in the previous tutorial.

Creating the Node

Now that we have our package, we need to write the code that will call the service to check whether a joint trajectory is valid. Fire up a text editor and paste the following into a file called src/get_joint_trajectory_validity.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 <planning_environment_msgs/GetRobotState.h>
   3 #include <planning_environment_msgs/GetJointTrajectoryValidity.h>
   4 
   5 int main(int argc, char **argv)
   6 {
   7   using namespace planning_environment_msgs; // for narrow wiki readablity
   8   ros::init (argc, argv, "get_trajectory_validity_test");
   9   ros::NodeHandle rh;
  10 
  11   std::string envServerName( "environment_server_right_arm" );
  12 
  13   ros::service::waitForService( envServerName + "/get_trajectory_validity");
  14 
  15   GetJointTrajectoryValidity::Request  req;
  16   GetJointTrajectoryValidity::Response res;
  17   ros::ServiceClient check_trajectory_validity_client_
  18                        = rh.serviceClient<GetJointTrajectoryValidity>(
  19                               envServerName + "/get_trajectory_validity");
  20 
  21   ros::service::waitForService( envServerName + "/get_robot_state");
  22   ros::ServiceClient get_state_client_
  23                        = rh.serviceClient<GetRobotState>(
  24                               envServerName + "/get_robot_state");      
  25 
  26 
  27   GetRobotState::Request  request;
  28   GetRobotState::Response response;
  29 
  30   if(get_state_client_.call(request,response))
  31   {
  32     req.robot_state = response.robot_state;
  33   }
  34   else
  35   {
  36     ROS_ERROR( "Service call to get robot state failed on %s",
  37                 get_state_client_.getService().c_str()         );
  38   }
  39 
  40   req.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  41   req.trajectory.points.resize(100);
  42   for(unsigned int i=0; i < 100; i++)
  43   {    
  44     req.trajectory.points[i].positions.push_back(-(3*M_PI*i/4.0)/100.0);
  45   }
  46 
  47   req.check_collisions = true;
  48 
  49   if(check_trajectory_validity_client_.call(req,res))
  50   {
  51     if(res.error_code.val == res.error_code.SUCCESS)
  52       ROS_INFO("Requested trajectory is not in collision");
  53     else
  54       ROS_INFO( "Requested trajectory is in collision. Error code: %d",
  55                 res.error_code.val                                      );
  56   }
  57   else
  58   {
  59     ROS_ERROR("Service call to check trajectory validity failed %s",
  60               check_trajectory_validity_client_.getService().c_str());
  61     return false;
  62   }
  63 
  64   ros::shutdown();
  65 }

Creating the service request

The service request to the GetJointTrajectoryValidity service requires the following information:

Filling up the joint trajectory

We will fill the joint trajectory only for a single joint, the r_arm_shoulder_pan_joint.

  27   GetRobotState::Request  request;
  28   GetRobotState::Response response;
  29 
  30   if(get_state_client_.call(request,response))
  31   {
  32     req.robot_state = response.robot_state;

Types of checks

The types of checks that can be performed on the input trajectory are specified within the service request itself.

  33   }

There are currently 4 types of checks that you can ask to be carried out. Each type of check can be activated by setting the corresponding flag.

There is also another flag for this message that allows you to specify whether the entire trajectory should be checked before the service call returns. By default, the service call returns as soon as one point in the trajectory is found to violate any of the specified tests.

Filling up the robot state

We have specified the desired trajectory for a single joint. We now need to specify the static/rest positions for the rest of the joints on the robot which will not be moving. The service request checks the robot state to make sure it contains joint values for every single joint on the robot. So, we will use a GetRobotState service to get the current state of the robot and fill up the robot_state field in our service request.

  14   GetJointTrajectoryValidity::Request  req;
  15   GetJointTrajectoryValidity::Response res;
  16   ros::ServiceClient check_trajectory_validity_client_
  17                        = rh.serviceClient<GetJointTrajectoryValidity>(
  18                               envServerName + "/get_trajectory_validity");
  19 
  20   ros::service::waitForService( envServerName + "/get_robot_state");
  21   ros::ServiceClient get_state_client_
  22                        = rh.serviceClient<GetRobotState>(
  23                               envServerName + "/get_robot_state");      

Interpreting the service response

After calling the service, we need to check the error code on the service response to determine whether our checks on the trajectory were successful.

  34   else
  35   {
  36     ROS_ERROR( "Service call to get robot state failed on %s",
  37                 get_state_client_.getService().c_str()         );
  38   }
  39 
  40   req.trajectory.joint_names.push_back("r_shoulder_pan_joint");
  41   req.trajectory.points.resize(100);
  42   for(unsigned int i=0; i < 100; i++)
  43   {    
  44     req.trajectory.points[i].positions.push_back(-(3*M_PI*i/4.0)/100.0);
  45   }

The service response returns two error codes, an overall error_code that indicates whether any point in the trajectory violates the requested checks and a vector of error codes that encode the status for each individual point in the requested trajectory. The vector of error_codes will be fully filled up only if the req.flag contains the CHECK_FULL_TRAJECTORY flag.

The overall error code will indicate SUCCESS if all the checks pass. e.g., if you are running collision tests, a response of SUCCESS indicates that all points in the trajectory are collision free. Note also that the checks are only carried out for individual points in the trajectory, no interpolation is carried out between consecutive points in the trajectory.

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/get_joint_trajectory_validity.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(get_joint_trajectory_validity src/get_joint_trajectory_validity.cpp)

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

make

Running in simulation

Follow the instructions for running in simulation in this previous tutorial, except launch rviz using the following command:

roslaunch pr2_arm_navigation_tutorials rviz_collision_tutorial_4.launch

Run get_joint_trajectory_validity

Next, make sure that the 10th rviz entry -- Collision Pose -- is enabled run the executable we just created

./bin/get_joint_trajectory_validity

If it succeeds, you should see the following output

[ INFO] 33.179000000: Requested trajectory is in collision. Error code: -23

The error code corresponds to the error codes in the ArmNavigationErrorCodes message in the motion_planning_msgs package.

You should see something like this in rviz:

rviz showing first collision pose

The Collision Pose display shows the last point in the trajectory that resulted in collision. As we did not select the check_full_trajectory option in the GetJointTrajectoryValidity message, the environment server stopped checking after getting any non valid poses - thus the shown pose is the first pose in the trajectory that resulted in collisions. Selecting the points will allow you see the links and objects in collision.


2019-12-14 12:49