## For instruction on writing tutorials
## http://www.ros.org/wiki/WritingTutorials
####################################
##FILL ME IN
####################################
## for a custom note with links:
## note = 
## for the canned note of "This tutorial assumes that you have completed the previous tutorials:" just add the links 
## note.0=[[arm_navigation/Tutorials/Planning Scene/Checking State Validity| Checking State Validity]] 
## descriptive title for the tutorial
## title = Checking Trajectory Validity
## multi-line description to be displayed in search 
## 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.
## the next tutorial description (optional)
## next =
## links to next tutorial (optional)
## next.0.link=
## next.1.link=
## what level user is this tutorial for 
## level= IntermediateCategory
## keywords =
####################################

<<IncludeCSTemplate(TutorialCSHeaderTemplate)>>

<<TableOfContents(4)>>

In the [[arm_navigation/Tutorials/Planning Scene/Checking State Validity| 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:

{{attachment:get_trajectory_validity_one.png | validity with goal = .9 | width=800}}

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:

{{attachment:get_trajectory_validity_two.png | validity with goal = .-.35 | width=800}}

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:

{{attachment:get_trajectory_validity_three.png | validity with goal = -.9 | width=800}}

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''.

{{{
#!cplusplus block=tree_block
#include <ros/ros.h>
#include <arm_navigation_msgs/GetPlanningScene.h>
#include <planning_environment/models/collision_models.h>

static const std::string SET_PLANNING_SCENE_DIFF_NAME = "/environment_server/set_planning_scene_diff";

int main(int argc, char **argv){
  ros::init (argc, argv, "get_state_validity_test");
  ros::NodeHandle rh;

  ros::Publisher vis_marker_publisher_;
  ros::Publisher vis_marker_array_publisher_;

  vis_marker_publisher_ = rh.advertise<visualization_msgs::Marker>("state_validity_markers", 128);
  vis_marker_array_publisher_ = rh.advertise<visualization_msgs::MarkerArray>("state_validity_markers_array", 128);

  ros::service::waitForService(SET_PLANNING_SCENE_DIFF_NAME);
  ros::ServiceClient get_planning_scene_client = 
    rh.serviceClient<arm_navigation_msgs::GetPlanningScene>(SET_PLANNING_SCENE_DIFF_NAME);

  arm_navigation_msgs::GetPlanningScene::Request planning_scene_req;
  arm_navigation_msgs::GetPlanningScene::Response planning_scene_res;

  if(!get_planning_scene_client.call(planning_scene_req, planning_scene_res)) {
    ROS_WARN("Can't get planning scene");
    return -1;
  }

  planning_environment::CollisionModels collision_models("robot_description");
  planning_models::KinematicState* state = 
    collision_models.setPlanningScene(planning_scene_res.planning_scene);

  std::vector<std::string> arm_names = 
    collision_models.getKinematicModel()->getModelGroup("right_arm")->getUpdatedLinkModelNames();
  std::vector<std::string> joint_names = 
    collision_models.getKinematicModel()->getModelGroup("right_arm")->getJointModelNames();

  trajectory_msgs::JointTrajectory trajectory;
  trajectory.joint_names.push_back("r_shoulder_pan_joint");
  trajectory.points.resize(100);

  std::map<std::string, double> start_vals;
  state->getKinematicStateValues(start_vals);
  double start_angle = start_vals["r_shoulder_pan_joint"];

  double goal_angle = .9;
  if(argc > 1) {
    std::stringstream s(argv[1]);
    s >> goal_angle;
  }

  for(unsigned int i=0; i < 100; i++)
  {    
    trajectory.points[i].positions.push_back(start_angle+i*(goal_angle-start_angle)/100.0);
  }

  std_msgs::ColorRGBA good_color, collision_color, joint_limits_color;
  good_color.a = collision_color.a = joint_limits_color.a = .8;

  good_color.g = 1.0;
  collision_color.r = 1.0;
  joint_limits_color.b = 1.0;

  std_msgs::ColorRGBA point_markers;
  point_markers.a = 1.0;
  point_markers.r = 1.0;
  point_markers.g = .8;

  visualization_msgs::MarkerArray arr;

  arm_navigation_msgs::ArmNavigationErrorCodes traj_code;
  std::vector<arm_navigation_msgs::ArmNavigationErrorCodes> traj_codes;
  arm_navigation_msgs::Constraints empty_constraints;

  std::map<std::string, double> state_vals;
  if(!collision_models.isJointTrajectoryValid(*state,
                                              trajectory,
                                              empty_constraints,
                                              empty_constraints,
                                              traj_code,
                                              traj_codes,
                                              false)) {
    std_msgs::ColorRGBA color;
    state_vals["r_shoulder_pan_joint"] = trajectory.points[traj_codes.size()-1].positions[0];
    state->setKinematicState(state_vals);
    if(traj_code.val == traj_code.JOINT_LIMITS_VIOLATED) {
      color = joint_limits_color;
    } else {
      color = collision_color;
      collision_models.getAllCollisionPointMarkers(*state,
                                                   arr,
                                                   point_markers,
                                                   ros::Duration(0.2));
    }
    collision_models.getRobotMarkersGivenState(*state,
                                               arr,
                                               color,
                                               "right_arm_invalid",
                                               ros::Duration(0.2),
                                               &arm_names);
  }
  state_vals["r_shoulder_pan_joint"] = trajectory.points.back().positions[0];
  state->setKinematicState(state_vals);
  collision_models.getRobotMarkersGivenState(*state,
                                             arr,
                                             good_color,
                                             "right_arm_goal",
                                             ros::Duration(0.2),
                                             &arm_names);

  while(ros::ok()) {    
    vis_marker_array_publisher_.publish(arr);
    ros::spinOnce();
    ros::Duration(.1).sleep();
  }
  collision_models.revertPlanningScene(state);
  ros::shutdown();
}

}}}

=== Understanding the code ===

The initial parts of the code look exactly like that in the previous tutorial.
The first differences come here:

<<CodeRef(tree_block, 38, 40)>>

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.

<<CodeRef(tree_block, 42, 50)>>

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.  

<<CodeRef(tree_block, 52, 55)>>

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.  

<<CodeRef(tree_block, 71, 73)>>

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.

<<CodeRef(tree_block, 76, 101)>>

Next we do the actual trajectory check using the [[http://www.ros.org/doc/electric/api/planning_environment/html/classplanning__environment_1_1CollisionModels.html|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. 

<<CodeRef(tree_block, 102, 109)>> 

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.

## AUTOGENERATED DO NOT DELETE 
## TutorialCategory
## FILL IN THE STACK TUTORIAL CATEGORY HERE