| Note: This tutorial assumes that you have completed the previous tutorials: Checking State Validity. |
 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 Trajectory Validity
Description: This tutorial will describe using the Planning Scene Architecture to check whether or not an entire Joint Trajectory is valid: focusing on whether it obeys joint limits and avoids collision.Tutorial Level: INTERMEDIATE
In the previous tutorial we focused on checking individual states for validity. In this tutorial we check trajectories, which for our purposes are a series of states. All states in a trajectory must be valid for the trajectory to be considered valid.
Setting up the tutorial
In one terminal run the following:
roslaunch pr2_arm_navigation_tutorials pr2_floorobj_world.launch
In another terminal run:
roslaunch pr2_arm_navigation_tutorials planning_scene_architecture.launch
This will bring up an Rviz window.
Running the tutorial
This tutorial uses the executable in pr2_arm_navigation_tutorials called get_joint_trajectory_validity. In another terminal, run the following command:
rosrun pr2_arm_navigation_tutorials get_joint_trajectory_validity
You should see something like this in Rviz:
Without an argument, this executable will check a trajectory for validiting that starts in the current configuration of the arm and moves the joint r_shoulder_pan_link to a position of .9 radians. The goal position is shown in green, and the first state in the trajectory that was found to be invalid in shown in red. In this case, the trajectory is invalid because it moves into contact with the left arm.
Now try this command (quitting with Ctrl-C first):
rosrun pr2_arm_navigation_tutorials get_joint_trajectory_validity -.35
You should see this:
In this case, we only see the green goal state. This trajectory was found to be valid. This means not only the goal state is valid - lying between the joint limits, and not in collision - but all the intermediate states of the trajectory.
Finally, try this after quitting the previous program:
rosrun pr2_arm_navigation_tutorials get_joint_trajectory_validity -.9
You should see this:
In this case despite the fact that the goal location is not in collision, the trajectory that takes us to that goal location results in a collision with the environment, with the first colliding state shown in red. Note that we do collision checking with padded robot meshes, so the unpadded links shown in the first colliding state may not look like they are actually in collision.
The code
You can find this code in the package pr2_arm_navigation_tutorials in the directory src/get_state_validity.cpp.
1 #include <ros/ros.h>
2 #include <arm_navigation_msgs/GetPlanningScene.h>
3 #include <planning_environment/models/collision_models.h>
4
5 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";
6
7 int main(int argc, char **argv){
8 ros::init (argc, argv, "get_state_validity_test");
9 ros::NodeHandle rh;
10
11 ros::Publisher vis_marker_publisher_;
12 ros::Publisher vis_marker_array_publisher_;
13
14 vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
15 vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);
16
17 ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
18 ros::ServiceClient get_planning_scene_client =
19 rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);
20
21 arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
22 arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;
23
24 if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
25 ROS_WARN("Can't get planning scene");
26 return -1;
27 }
28
29 planning_environment::CollisionModels collision_models("robot_description");
30 planning_models::KinematicState* state =
31 collision_models.setPlanningScene(planning_scene_res.planning_scene);
32
33 std::vector<std::string> arm_names =
34 collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
35 std::vector<std::string> joint_names =
36 collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();
37
38 trajectory_msgs::JointTrajectory trajectory;
39 trajectory.joint_names.push_back("r_shoulder_pan_joint");
40 trajectory.points.resize(100);
41
42 std::map<std::string, double> start_vals;
43 state->getKinematicStateValues(start_vals);
44 double start_angle = start_vals["r_shoulder_pan_joint"];
45
46 double goal_angle = .9;
47 if(argc > 1) {
48 std::stringstream s(argv[1]);
49 s >> goal_angle;
50 }
51
52 for(unsigned int i=0; i < 100; i++)
53 {
54 trajectory.points[i].positions.push_back(start_angle+i*(goal_angle-start_angle)/100.0);
55 }
56
57 std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
58 good_color.a = collision_color.a = joint_limits_color.a = .8;
59
60 good_color.g = 1.0;
61 collision_color.r = 1.0;
62 joint_limits_color.b = 1.0;
63
64 std_msgs::ColorRGBA point_markers;
65 point_markers.a = 1.0;
66 point_markers.r = 1.0;
67 point_markers.g = .8;
68
69 visualization_msgs::MarkerArray arr;
70
71 arm_navigation_msgs::ArmNavigationErrorCodes traj_code;
72 std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_codes;
73 arm_navigation_msgs::Constraints empty_constraints;
74
75 std::map<std::string, double> state_vals;
76 if(!collision_models.isJointTrajectoryValid(*state,
77 trajectory,
78 empty_constraints,
79 empty_constraints,
80 traj_code,
81 traj_codes,
82 false)) {
83 std_msgs::ColorRGBA color;
84 state_vals["r_shoulder_pan_joint"] = trajectory.points[traj_codes.size()-1].positions[0];
85 state->setKinematicState(state_vals);
86 if(traj_code.val == traj_code.JOINT_LIMITS_VIOLATED) {
87 color = joint_limits_color;
88 } else {
89 color = collision_color;
90 collision_models.getAllCollisionPointMarkers(*state,
91 arr,
92 point_markers,
93 ros::Duration(0.2));
94 }
95 collision_models.getRobotMarkersGivenState(*state,
96 arr,
97 color,
98 "right_arm_invalid",
99 ros::Duration(0.2),
100 &arm_names);
101 }
102 state_vals["r_shoulder_pan_joint"] = trajectory.points.back().positions[0];
103 state->setKinematicState(state_vals);
104 collision_models.getRobotMarkersGivenState(*state,
105 arr,
106 good_color,
107 "right_arm_goal",
108 ros::Duration(0.2),
109 &arm_names);
110
111 while(ros::ok()) {
112 vis_marker_array_publisher_.publish(arr);
113 ros::spinOnce();
114 ros::Duration(.1).sleep();
115 }
116 collision_models.revertPlanningScene(state);
117 ros::shutdown();
118 }
Understanding the code
The initial parts of the code look exactly like that in the previous tutorial. The first differences come here:
Here we are composing a joint trajectory for a single joint - the r_shoulder_pan_joint. The joint trajectory will have 100 positions of this single joint.
Next we set start and goal joint angles. We read the start angle for the joint from the Planning Scene state, and set the goal angle to .9 unless an argument is supplied.
Next we set the trajectory points to evenly interpolate 100 points between the start angle and the goal angle - by selecting a fine discretization we can have a relatively high degree of confidence that any collision along the path will be detected.
After we set up the markers and colors we create the data structures that we'll use for the validity check. Especially important to note is that we are setting up empty constraints. The validity check can include both goal and path constraints. Goal constraints allow us to check that the trajectory actually gets us to a goal location - that the final point on the trajectory is within a particular workspace region or near particular joint constraints. Path constraints allow us to check that each point on the trajectory obeys particular constraints. We won't explore these uses of the trajectory validity check further in this tutorial.
76 if(!collision_models.isJointTrajectoryValid(*state,
77 trajectory,
78 empty_constraints,
79 empty_constraints,
80 traj_code,
81 traj_codes,
82 false)) {
83 std_msgs::ColorRGBA color;
84 state_vals["r_shoulder_pan_joint"] = trajectory.points[traj_codes.size()-1].positions[0];
85 state->setKinematicState(state_vals);
86 if(traj_code.val == traj_code.JOINT_LIMITS_VIOLATED) {
87 color = joint_limits_color;
88 } else {
89 color = collision_color;
90 collision_models.getAllCollisionPointMarkers(*state,
91 arr,
92 point_markers,
93 ros::Duration(0.2));
94 }
95 collision_models.getRobotMarkersGivenState(*state,
96 arr,
97 color,
98 "right_arm_invalid",
99 ros::Duration(0.2),
100 &arm_names);
101 }
Next we do the actual trajectory check using the CollisionModels API. The function will return true only if every single state is judged to be valid. Otherwise it will return false, and set an appropriate error code in the traj_code argument. It will also set values for each individual trajectory point in the traj_codes argument. We also set the final argument of the function to false, which will cause the function to return as soon as the first invalid trajectory point is discovered instead of continuing to check the entire trajectory. With this setting, the final entry in traj_codes will be for the point that was actually invalid. We produce markers for this state, and show it in red in Rviz.
Finally, we produce a set of green markers for the goal state even if some trajectory point was found to be invalid, and publish all the markers to Rviz.
