| 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
Contents
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:
- A joint trajectory that you would like to check
- The types of checks to be carried out
- A robot state that contains a complete specification for the state for the rest of the joints on the robot
Filling up the joint trajectory
We will fill the joint trajectory only for a single joint, the r_arm_shoulder_pan_joint.
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.
collision checks: request.check_collisions = true;
joint limits checks: request.check_joint_limits = true;
goal constraints test: request.check_goal_constraints = true;
path constraints test: request.check_path_constraints = true;
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.
check full trajectory: request.check_full_trajectory = true;
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:
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.
