planning_environment: planning_monitor.cpp Source File

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 * 
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 * 
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 * 
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 * 
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #include "planning_environment/monitors/planning_monitor.h"
00038 #include "planning_environment/util/kinematic_state_constraint_evaluator.h"
00039 #include <boost/scoped_ptr.hpp>
00040 #include <motion_planning_msgs/DisplayTrajectory.h>
00041 
00042 void planning_environment::PlanningMonitor::loadParams(void)
00043 {
00044   nh_.param<double>("collision_map_safety_timeout", intervalCollisionMap_, 0.0);
00045   nh_.param<double>("joint_states_safety_timeout", intervalState_, 0.0);
00046   nh_.param<double>("tf_safety_timeout", intervalPose_, 0.0);
00047   nh_.param<int>("contacts_to_compute_for_allowable_contacts_test", num_contacts_allowable_contacts_test_, 10);
00048   nh_.param<int>("contacts_to_compute_for_display", num_contacts_for_display_, 1);
00049 
00050   display_collision_pose_publisher_ = nh_.advertise<motion_planning_msgs::DisplayTrajectory>("collision_pose", 1);
00051   display_state_validity_publisher_ = nh_.advertise<motion_planning_msgs::DisplayTrajectory>("state_validity", 1);
00052 }
00053 
00054 bool planning_environment::PlanningMonitor::prepareForValidityChecks(const std::vector<std::string>& joint_names,
00055                                                                      const motion_planning_msgs::OrderedCollisionOperations& ordered_collision_operations,
00056                                                                      const std::vector<motion_planning_msgs::AllowedContactSpecification>& allowed_contacts,
00057                                                                      const motion_planning_msgs::Constraints& path_constraints,
00058                                                                      const motion_planning_msgs::Constraints& goal_constraints,
00059                                                                      const std::vector<motion_planning_msgs::LinkPadding>& link_padding,
00060                                                                      motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00061 {
00062   getEnvironmentModel()->lock();
00063 
00064   //copying collision map filtered for static objects into the collision space
00065   setCollisionSpace();
00066 
00067   //changing any necessary link paddings, which destroys and recreates the objects
00068   applyLinkPaddingToCollisionSpace(link_padding);
00069  
00070   for(unsigned int i  = 0; i < joint_names.size(); i++) {
00071     ROS_DEBUG_STREAM("Parent joint " << joint_names[i]);
00072   }
00073 
00074   //turning off collisions except for a set of specified joints, after which any other requested ordered collision operations will be preformed
00075   motion_planning_msgs::OrderedCollisionOperations operations;
00076   std::vector<std::string> child_links;
00077   getChildLinks(joint_names, child_links);
00078   for(unsigned int i  = 0; i < child_links.size(); i++) {
00079     ROS_DEBUG_STREAM("Child link " << child_links[i]);
00080   }
00081 
00082   getOrderedCollisionOperationsForOnlyCollideLinks(child_links,ordered_collision_operations,operations);
00083   applyOrderedCollisionOperationsToCollisionSpace(operations);
00084 
00085   //setting any allowed contacts
00086   setAllowedContacts(allowed_contacts);
00087   
00088   //setting path and goal constraints, which may fail due to frame transform issues  
00089   if(!setPathConstraints(path_constraints, error_code)) {
00090     return false;
00091   }
00092 
00093   if(!setGoalConstraints(goal_constraints, error_code)) {
00094     return false;
00095   }
00096   return true;
00097 }
00098 
00099 void planning_environment::PlanningMonitor::revertToDefaultState() {
00100   revertAllowedCollisionToDefault();
00101   revertCollisionSpacePaddingToDefault();
00102   clearAllowedContacts();
00103   clearConstraints();
00104   getEnvironmentModel()->unlock();
00105 }
00106 
00107 bool planning_environment::PlanningMonitor::isEnvironmentSafe(motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00108 {
00109 
00110   if (use_collision_map_ && (!haveMap_ || !isMapUpdated(intervalCollisionMap_)))
00111   {
00112     ROS_WARN("Environment is not safe for motion: Collision map not updated in the last %f seconds", intervalCollisionMap_);
00113     error_code.val = error_code.SENSOR_INFO_STALE;
00114     return false;
00115   }
00116   
00117   if (!isJointStateUpdated(intervalState_))
00118   {
00119     ROS_WARN("Environment is not safe for motion: Robot state not updated in the last %f seconds", intervalState_);
00120     error_code.val = error_code.ROBOT_STATE_STALE;
00121     return false;
00122   }  
00123 
00124   if (!isPoseUpdated(intervalPose_))
00125   {
00126     ROS_WARN("Environment is not safe for motion: Robot pose not updated in the last %f seconds", intervalPose_);
00127     error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00128     return false;
00129   }
00130   
00131   error_code.val = error_code.SUCCESS;
00132   return true;
00133 }
00134 
00135 void planning_environment::PlanningMonitor::clearConstraints(void)
00136 {
00137   path_constraints_.joint_constraints.clear();
00138   path_constraints_.position_constraints.clear();
00139   path_constraints_.orientation_constraints.clear();
00140   path_constraints_.visibility_constraints.clear();
00141 
00142   goal_constraints_.joint_constraints.clear();
00143   goal_constraints_.position_constraints.clear();
00144   goal_constraints_.orientation_constraints.clear();
00145   goal_constraints_.visibility_constraints.clear();
00146 }
00147 
00148 bool planning_environment::PlanningMonitor::setPathConstraints(const motion_planning_msgs::Constraints &constraints, 
00149                                                                motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00150 {
00151   path_constraints_ = constraints;
00152   return transformConstraintsToFrame(path_constraints_, getWorldFrameId(), error_code);
00153 }
00154 
00155 bool planning_environment::PlanningMonitor::setGoalConstraints(const motion_planning_msgs::Constraints &constraints,
00156                                                                motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00157 {
00158   goal_constraints_ = constraints;
00159   return transformConstraintsToFrame(goal_constraints_, getWorldFrameId(), error_code);
00160 }
00161 
00162 bool planning_environment::PlanningMonitor::transformConstraintsToFrame(motion_planning_msgs::Constraints &constraints, 
00163                                                                         const std::string &target, 
00164                                                                         motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00165 {
00166   geometry_msgs::PointStamped pos;
00167   geometry_msgs::PoseStamped tmp_pose;
00168   for (unsigned int i = 0; i < constraints.position_constraints.size() ; ++i)
00169   {
00170     bool ok = false;    
00171     unsigned int steps = 0;
00172     while (!tf_->canTransform(target, constraints.position_constraints[i].header.frame_id,ros::Time()) && steps < 10)
00173     {
00174       ros::Duration(0.01).sleep();
00175       steps++;
00176     }   
00177     try
00178     {
00179       pos.point = constraints.position_constraints[i].position;
00180       pos.header = constraints.position_constraints[i].header;
00181       pos.header.stamp = ros::Time();
00182       tf_->transformPoint(target,pos,pos);
00183       constraints.position_constraints[i].position = pos.point;
00184 
00185       ROS_DEBUG_STREAM("Transforming position constraint from frame " << target << " from frame " << pos.header.frame_id);
00186       ROS_DEBUG_STREAM("Resulting position is " << pos.point.x << " " << pos.point.y << " " << pos.point.z);
00187 
00188       geometry_msgs::QuaternionStamped tmp_quaternion;
00189       tmp_quaternion.quaternion = constraints.position_constraints[i].constraint_region_orientation;
00190       tmp_quaternion.header = constraints.position_constraints[i].header;
00191       tmp_quaternion.header.stamp = ros::Time();
00192       tf_->transformQuaternion(target,tmp_quaternion,tmp_quaternion);
00193       constraints.position_constraints[i].header.frame_id = tmp_quaternion.header.frame_id;
00194       constraints.position_constraints[i].header.stamp = ros::Time::now();
00195       ok = true;
00196     }
00197     catch(...)
00198     {
00199     }
00200         
00201     if (!ok)
00202     {
00203       ROS_ERROR("Unable to transform pose constraint on link '%s' to frame '%s'", constraints.position_constraints[i].link_name.c_str(), target.c_str());
00204       error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00205       return false;
00206     }
00207   }
00208 
00209   for (unsigned int i = 0; i < constraints.orientation_constraints.size() ; ++i)
00210   {
00211     bool ok = false;    
00212     unsigned int steps = 0;
00213     while (!tf_->canTransform(target, constraints.orientation_constraints[i].header.frame_id, ros::Time()) && steps < 10)
00214     {
00215       ros::Duration(0.01).sleep();
00216       steps++;
00217     }
00218     try
00219     {
00220       geometry_msgs::QuaternionStamped orientation;
00221       orientation.header = constraints.orientation_constraints[i].header;
00222       orientation.header.stamp = ros::Time();
00223       orientation.quaternion = constraints.orientation_constraints[i].orientation;
00224 
00225       tf_->transformQuaternion(target, orientation, orientation);
00226       constraints.orientation_constraints[i].orientation = orientation.quaternion;
00227       constraints.orientation_constraints[i].header = orientation.header;
00228       ok = true;
00229     }
00230     catch(...)
00231     {
00232     }
00233         
00234     if (!ok)
00235     {
00236       ROS_ERROR("Unable to transform pose constraint on link '%s' to frame '%s'", constraints.orientation_constraints[i].link_name.c_str(), target.c_str());
00237       error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00238       return false;
00239     }
00240   }    
00241   // if there are any floating or planar joints, transform them
00242   //TODO - figure out what to do here
00243   /*
00244     if (getKinematicModel()->getPlanarJoints().size() > 0 || getKinematicModel()->getFloatingJoints().size() > 0)
00245     {
00246     for (unsigned int i = 0; i < constraints.joint_constraints.size() ; ++i)
00247     if (!transformJoint(constraints.joint_constraints[i].joint_name, 
00248     0, constraints.joint_constraints[i].position, 
00249     constraints.joint_constraints[i].header, target))
00250     {
00251     error_code.val = error_code.FRAME_TRANSFORM_FAILURE;        
00252     return false;
00253     }
00254   }
00255   */
00256   for (unsigned int i = 0; i < constraints.visibility_constraints.size() ; ++i)
00257   {
00258     bool ok = false;    
00259     geometry_msgs::PointStamped point = constraints.visibility_constraints[i].target;
00260     point.header.stamp = ros::Time();
00261     try
00262     {
00263       tf_->transformPoint(target, point, point);
00264       constraints.visibility_constraints[i].target = point;
00265       ok = true;
00266     }
00267     catch(...)
00268     {
00269     }
00270     if (!ok)
00271     {
00272       ROS_ERROR("Unable to transform visibility constraint on link '%s' to frame '%s'", constraints.visibility_constraints[i].target.header.frame_id.c_str(), target.c_str());
00273       error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00274       return false;
00275     }
00276   }    
00277   error_code.val = error_code.SUCCESS;
00278   return true;
00279 }
00280 
00281 bool planning_environment::PlanningMonitor::transformTrajectoryToFrame(trajectory_msgs::JointTrajectory &kp, 
00282                                                                        motion_planning_msgs::RobotState &robot_state, 
00283                                                                        const std::string &target, 
00284                                                                        motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00285 {    
00286   //TODO - here too
00287   // if there are no planar of floating transforms, there is nothing to do
00288   //  if (getKinematicModel()->getPlanarJoints().empty() && getKinematicModel()->getFloatingJoints().empty())
00289   //   {
00290   //     kp.header.frame_id = target;
00291   //     return true;
00292   //   }    
00293   //  roslib::Header updatedHeader = kp.header;
00294   std::string updated_frame_id = target;
00295     
00296   // transform start state
00297   for (unsigned int i = 0 ; i < robot_state.joint_state.position.size() ; ++i)
00298     if (!transformJointToFrame(robot_state.joint_state.position[i], robot_state.joint_state.name[i], kp.header.frame_id, target, error_code))
00299       return false;
00300     
00301     
00302   // transform the rest of the states
00303 
00304   // get the joints this path is for
00305   std::vector<const planning_models::KinematicModel::JointModel*> joints;
00306   joints.resize(kp.joint_names.size());
00307   for (unsigned int j = 0 ; j < joints.size() ; ++j)
00308   {
00309     joints[j] = getKinematicModel()->getJointModel(kp.joint_names[j]);
00310     if (joints[j] == NULL)
00311     {
00312       ROS_ERROR("Unknown joint '%s' found on path", kp.joint_names[j].c_str());
00313       error_code.val = error_code.INVALID_TRAJECTORY;
00314       return false;
00315     }
00316   }
00317     
00318   //TODO - here too
00319   // iterate through the states
00320   //  for (unsigned int i = 0 ; i < kp.points.size() ; ++i)
00321   //   {
00322   //     unsigned int u = 0;
00323   //     for (unsigned int j = 0 ; j < joints.size() ; ++j)
00324   //     {
00325   //       roslib::Header header = kp.header;
00326   //       if (!transformJoint(joints[j]->name, u, kp.points[i].positions[j], header, target, error_code))
00327   //       {
00328   //         error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00329   //         return false;
00330   //       }
00331   //       updatedHeader = header;
00332   //     }
00333   //   }
00334     
00335   kp.header.frame_id = updated_frame_id;
00336   return true;
00337 }
00338 
00339 bool planning_environment::PlanningMonitor::transformJointToFrame(double &value, 
00340                                                                   const std::string &joint_name, 
00341                                                                   std::string &frame_id, 
00342                                                                   const std::string &target,
00343                                                                   motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00344 {
00345   return transformJoint(joint_name, 0, value, frame_id, target, error_code);
00346 }
00347 
00348 bool planning_environment::PlanningMonitor::transformJoint(const std::string &name, 
00349                                                            unsigned int index, 
00350                                                            double &param, 
00351                                                            std::string &frame_id, 
00352                                                            const std::string& target, 
00353                                                            motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00354 {
00355   // TODO
00356   // planar joints and floating joints may need to be transformed 
00357   const planning_models::KinematicModel::JointModel *joint = getKinematicModel()->getJointModel(name);
00358   if (joint == NULL)
00359   {
00360     ROS_ERROR("Unknown joint '%s'", name.c_str());
00361     error_code.val = error_code.INVALID_TRAJECTORY;
00362     return false;
00363   }    
00364   frame_id = target;
00365   return true;
00366 }
00367 
00368 bool planning_environment::PlanningMonitor::isStateValid(const motion_planning_msgs::RobotState &robot_state, 
00369                                                          const int test, 
00370                                                          bool verbose, 
00371                                                          motion_planning_msgs::ArmNavigationErrorCodes &error_code)
00372 {   
00373   planning_models::KinematicState state(getKinematicModel());
00374 
00375   //setting the robot's configuration
00376   setRobotStateAndComputeTransforms(robot_state, state);
00377   getEnvironmentModel()->updateRobotModel(&state);
00378 
00379   bool valid = true;
00380     
00381   bool vlevel = getEnvironmentModel()->getVerbose();
00382   getEnvironmentModel()->setVerbose(verbose);
00383 
00384   motion_planning_msgs::DisplayTrajectory d_path;
00385   convertKinematicStateToRobotState(state, d_path.robot_state);
00386   d_path.trajectory.joint_trajectory.header = d_path.robot_state.joint_state.header;
00387   d_path.trajectory.joint_trajectory.joint_names = d_path.robot_state.joint_state.name;
00388   d_path.trajectory.joint_trajectory.points.resize(1);
00389   d_path.trajectory.joint_trajectory.points[0].positions = d_path.robot_state.joint_state.position;
00390   display_state_validity_publisher_.publish(d_path);
00391 
00392   std::vector<collision_space::EnvironmentModel::Contact> contacts;
00393   if (test & COLLISION_TEST) {
00394     unsigned int numContacts = num_contacts_for_display_;
00395     if(!allowedContacts_.empty()) {
00396       numContacts = num_contacts_allowable_contacts_test_;
00397     }
00398     valid = !getEnvironmentModel()->getCollisionContacts(allowedContacts_, contacts, numContacts);    
00399     // If a callback exists for contact determination, call the callback for each of the found contacts                                                     
00400     if (onCollisionContact_) {
00401       for (unsigned int i = 0 ; i < contacts.size() ; ++i)
00402         onCollisionContact_(contacts[i]);
00403     }
00404     
00405     if(!valid) 
00406     {
00407       error_code.val = error_code.COLLISION_CONSTRAINTS_VIOLATED;
00408       if(verbose)
00409         ROS_ERROR("State is in collision.\n");
00410       return false;
00411     }
00412   }
00413 
00414   //
00415   // Check the joint limits
00416   //
00417   if (test & JOINT_LIMITS_TEST)
00418   {
00419     bool within_bounds = state.areJointsWithinBounds(robot_state.joint_state.name);      
00420     if(!within_bounds)
00421     {
00422       error_code.val = error_code.JOINT_LIMITS_VIOLATED;
00423       if(verbose)
00424         ROS_WARN("Joint limits violated.\n");
00425       return false;
00426     }
00427   }
00428 
00429 
00430   //
00431   // Check the path constraints
00432   //
00433   if (test & PATH_CONSTRAINTS_TEST)
00434   {         
00435     valid = checkPathConstraints(&state,true);
00436     if(!valid) 
00437     {
00438       error_code.val = error_code.PATH_CONSTRAINTS_VIOLATED;
00439       if(verbose)
00440         ROS_WARN("State violates path constraints.\n");
00441       return false;
00442     }
00443   }
00444 
00445 
00446   //
00447   // Check the goal constraints
00448   //
00449   if (test & GOAL_CONSTRAINTS_TEST)
00450   {         
00451     ROS_DEBUG("Evaluating goal constraints: joint: %u, position: %u, orientation: %u",
00452               (unsigned int)goal_constraints_.joint_constraints.size(),
00453               (unsigned int)goal_constraints_.position_constraints.size(),
00454               (unsigned int)goal_constraints_.orientation_constraints.size());
00455 
00456     valid = checkGoalConstraints(&state,true);
00457     if(!valid) 
00458     {
00459       error_code.val = error_code.GOAL_CONSTRAINTS_VIOLATED;
00460       if(verbose)
00461         ROS_WARN("State violates goal constraints.\n");
00462       return false;
00463     }
00464   }    
00465   getEnvironmentModel()->setVerbose(vlevel);
00466   return valid;    
00467 }
00468 
00469 int planning_environment::PlanningMonitor::closestStateOnTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00470                                                                     motion_planning_msgs::RobotState &robot_state, 
00471                                                                     motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00472 {
00473   return closestStateOnTrajectory(trajectory, robot_state, 0, trajectory.points.size() - 1, error_code);
00474 }
00475 
00476 int planning_environment::PlanningMonitor::closestStateOnTrajectory(const trajectory_msgs::JointTrajectory &trajectory, 
00477                                                                     motion_planning_msgs::RobotState &robot_state, 
00478                                                                     unsigned int start, 
00479                                                                     unsigned int end, 
00480                                                                     motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00481 {
00482   if (end >= trajectory.points.size())
00483     end = trajectory.points.size() - 1;
00484   if (start > end)
00485   {
00486     ROS_ERROR("Invalid start %d and end %d specification",start,end);
00487     error_code.val = error_code.INVALID_INDEX;
00488     return -1;
00489   }
00490 
00491   if (trajectory.header.frame_id != getWorldFrameId())
00492   {
00493     trajectory_msgs::JointTrajectory pathT = trajectory;
00494     if (transformTrajectoryToFrame(pathT, robot_state, getWorldFrameId(), error_code))
00495       return closestStateOnTrajectoryAux(pathT, start, end, error_code);
00496     else
00497     {
00498       ROS_ERROR("Could not transform trajectory from %s to %s",trajectory.header.frame_id.c_str(),getWorldFrameId().c_str());
00499       error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00500       return -1;
00501     }
00502   }
00503   else
00504     return closestStateOnTrajectoryAux(trajectory, start, end, error_code);  
00505 }
00506 
00507 int planning_environment::PlanningMonitor::closestStateOnTrajectoryAux(const trajectory_msgs::JointTrajectory &trajectory, 
00508                                                                        unsigned int start, 
00509                                                                        unsigned int end, 
00510                                                                        motion_planning_msgs::ArmNavigationErrorCodes &error_code) const
00511 {
00512   double dist = 0.0;
00513   int    pos  = -1;
00514 
00515   std::map<std::string, double> current_joint_vals = getCurrentJointStateValues();
00516 
00517   for(unsigned int i = 0; i < trajectory.joint_names.size(); i++) {
00518     if(current_joint_vals.find(trajectory.joint_names[i]) == current_joint_vals.end()) {
00519       ROS_ERROR("Unknown joint '%s' found on path", trajectory.joint_names[i].c_str());
00520       error_code.val = error_code.INVALID_TRAJECTORY;
00521       return -1;
00522     }
00523   }
00524 
00525   for (unsigned int i = start ; i <= end ; ++i)
00526   {
00527     double d = 0.0;
00528     for (unsigned int j = 0 ; j < trajectory.joint_names.size(); ++j)
00529     {
00530       double current_joint_position = current_joint_vals.find(trajectory.joint_names[j])->second;
00531       double diff = fabs(trajectory.points[i].positions[j] - current_joint_position);
00532       d += diff * diff;
00533     }
00534         
00535     if (pos < 0 || d < dist)
00536     {
00537       pos = i;
00538       dist = d;
00539     }
00540   }    
00541   return pos;
00542 }
00543 
00544 
00545 
00546 bool planning_environment::PlanningMonitor::isTrajectoryValid(const trajectory_msgs::JointTrajectory &trajectory, 
00547                                                               motion_planning_msgs::RobotState &robot_state, 
00548                                                               const int test, 
00549                                                               bool verbose, 
00550                                                               motion_planning_msgs::ArmNavigationErrorCodes &error_code, 
00551                                                               std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes)
00552 {
00553   return isTrajectoryValid(trajectory, 
00554                            robot_state, 
00555                            0, 
00556                            trajectory.points.size() - 1, 
00557                            test, 
00558                            verbose,
00559                            error_code,trajectory_error_codes);
00560 }
00561 
00562 bool planning_environment::PlanningMonitor::isTrajectoryValid(const trajectory_msgs::JointTrajectory &trajectory, 
00563                                                               motion_planning_msgs::RobotState &robot_state, 
00564                                                               unsigned int start, 
00565                                                               unsigned int end, 
00566                                                               const int test, 
00567                                                               bool verbose,
00568                                                               motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00569                                                               std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes)
00570 {
00571   if (end >= trajectory.points.size())
00572     end = trajectory.points.size() - 1;
00573   if (start > end)
00574   {
00575     ROS_ERROR("Invalid trajectory: start: %d, end: %d",start,end);
00576     error_code.val = error_code.INVALID_INDEX;
00577     return true;
00578   }
00579   if (trajectory.header.frame_id != getWorldFrameId())
00580   {
00581     trajectory_msgs::JointTrajectory pathT = trajectory;
00582 
00583     if (transformTrajectoryToFrame(pathT, robot_state, getWorldFrameId(), error_code))
00584       return isTrajectoryValidAux(pathT, robot_state, start, end, test, verbose, error_code, trajectory_error_codes);
00585     else
00586     { 
00587       ROS_WARN("Could not transform trajectory from frame: %s to frame: %s",pathT.header.frame_id.c_str(),getWorldFrameId().c_str());
00588       error_code.val = error_code.FRAME_TRANSFORM_FAILURE;
00589       return false;
00590     }
00591   }
00592   else
00593     return isTrajectoryValidAux(trajectory, robot_state, start, end, test, verbose, error_code, trajectory_error_codes); 
00594 }
00595 
00596 bool planning_environment::PlanningMonitor::isTrajectoryValidAux(const trajectory_msgs::JointTrajectory &trajectory, 
00597                                                                  motion_planning_msgs::RobotState &robot_state, 
00598                                                                  unsigned int start, 
00599                                                                  unsigned int end, 
00600                                                                  const int test, 
00601                                                                  bool verbose,
00602                                                                  motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00603                                                                  std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes)
00604 {  
00605   
00606   planning_models::KinematicState state(getKinematicModel());
00607   
00608   //setting the robot's configuration
00609   setRobotStateAndComputeTransforms(robot_state, state);
00610   getEnvironmentModel()->updateRobotModel(&state);
00611 
00612   bool vlevel = getEnvironmentModel()->getVerbose();
00613   getEnvironmentModel()->setVerbose(verbose);
00614   
00615   bool valid = true;
00616     
00617   //joint map for setting trajectories
00618   std::map<std::string, double> joint_value_map;
00619 
00620   // get the joints this trajectory is for
00621   std::vector<planning_models::KinematicState::JointState*> joints(trajectory.joint_names.size());
00622   for (unsigned int j = 0 ; j < joints.size() ; ++j)
00623   {
00624     joints[j] = state.getJointState(trajectory.joint_names[j]);
00625     if (joints[j] == NULL)
00626     {
00627       ROS_ERROR("Unknown joint '%s' found on path", trajectory.joint_names[j].c_str());
00628       error_code.val = error_code.INVALID_TRAJECTORY;
00629       return false;
00630     } else {
00631       joint_value_map[joints[j]->getName()] = 0.0;
00632     }
00633   }
00634 
00635   //unsigned int remainingContacts = maxCollisionContacts_;    
00636   // check every state
00637   trajectory_error_codes.resize(trajectory.points.size());
00638   bool check_all = (test & CHECK_FULL_TRAJECTORY);
00639   bool valid_all = true;
00640   for (unsigned int i = start ; i <= end ; ++i)
00641   {
00642     if (trajectory.points[i].positions.size() != joints.size())
00643     {
00644       ROS_ERROR("Incorrect state specification on trajectory");
00645       error_code.val = error_code.INVALID_TRAJECTORY;      
00646       trajectory_error_codes[i].val = error_code.INVALID_TRAJECTORY;
00647       valid = false;
00648       valid_all = valid && valid_all;
00649       if(check_all)
00650         continue;
00651       else
00652         break;
00653     }
00654     for (unsigned int j = 0 ; j < trajectory.points[i].positions.size(); j++)
00655     {
00656       joint_value_map[trajectory.joint_names[j]] = trajectory.points[i].positions[j];
00657     }
00658     state.setKinematicState(joint_value_map);
00659     getEnvironmentModel()->updateRobotModel(&state);
00660 
00661     //
00662     // check the joint limits
00663     //
00664     if (test & JOINT_LIMITS_TEST)
00665     {
00666       valid = state.areJointsWithinBounds(trajectory.joint_names);
00667       valid_all = valid && valid_all;
00668       if(!valid)
00669       {
00670         error_code.val = error_code.JOINT_LIMITS_VIOLATED;
00671         trajectory_error_codes[i].val = error_code.JOINT_LIMITS_VIOLATED;
00672         ROS_ERROR("Joint limits violated");
00673         if(check_all)
00674           continue;
00675         else
00676           break;
00677       }
00678     }
00679 
00680     //
00681     // check for collisions
00682     //
00683     std::vector<collision_space::EnvironmentModel::Contact> contacts;
00684     if (test & COLLISION_TEST) 
00685     {
00686       unsigned int numContacts = num_contacts_for_display_;
00687       if(!allowedContacts_.empty()) {
00688         numContacts = num_contacts_allowable_contacts_test_;
00689       }
00690       valid = !getEnvironmentModel()->getCollisionContacts(allowedContacts_, contacts, numContacts);    
00691       
00692       if (onCollisionContact_) {
00693         for (unsigned int i = 0 ; i < contacts.size() ; ++i)
00694           onCollisionContact_(contacts[i]);
00695       }
00696       
00697       valid_all = valid && valid_all;
00698     }
00699     if(!valid)
00700     {
00701       //        if(!this->broadcastCollisions())
00702       //          ROS_WARN("Could not broadcast solutions");
00703       ROS_DEBUG("Found a collision for trajectory index: %d",i);
00704 
00705       motion_planning_msgs::DisplayTrajectory d_path;
00706       d_path.trajectory.joint_trajectory.header = robot_state.joint_state.header;
00707       d_path.trajectory.joint_trajectory.joint_names = trajectory.joint_names;
00708       d_path.trajectory.joint_trajectory.points.resize(1);
00709       d_path.trajectory.joint_trajectory.points[0] = trajectory.points[i];
00710       d_path.robot_state = robot_state;
00711       display_collision_pose_publisher_.publish(d_path);
00712 
00713       error_code.val = error_code.COLLISION_CONSTRAINTS_VIOLATED;
00714       trajectory_error_codes[i].val = error_code.COLLISION_CONSTRAINTS_VIOLATED;
00715       if(check_all)
00716         continue;
00717       else
00718         break;
00719     }
00720     //
00721     // check against the path constraints
00722     //
00723     if (test & PATH_CONSTRAINTS_TEST)
00724     {
00725       valid = checkPathConstraints(&state, verbose);
00726       valid_all = valid && valid_all;
00727       if (!valid)
00728       {
00729         if(verbose)
00730           ROS_INFO("isTrajectoryValid: State %d does not satisfy path constraints", i);
00731         error_code.val = error_code.PATH_CONSTRAINTS_VIOLATED;
00732         trajectory_error_codes[i].val = error_code.COLLISION_CONSTRAINTS_VIOLATED;
00733         if(check_all)
00734           continue;
00735         else
00736           break;
00737       }
00738     }       
00739   }
00740 
00741   //
00742   // check against the goal constraints
00743   //
00744   if (valid_all && (test & GOAL_CONSTRAINTS_TEST))
00745   {
00746     valid = checkGoalConstraints(&state, verbose);
00747     valid_all = valid && valid_all;
00748     if (!valid)
00749     {
00750       ROS_WARN("isTrajectoryValid: Goal state does not satisfy goal constraints");
00751       error_code.val = error_code.GOAL_CONSTRAINTS_VIOLATED;
00752     }
00753   }
00754 
00755   getEnvironmentModel()->setVerbose(vlevel);
00756   return valid_all;
00757 }
00758 
00759 bool planning_environment::PlanningMonitor::broadcastCollisions() {
00760   
00761   unsigned int numContacts = num_contacts_for_display_;
00762   if(!allowedContacts_.empty()) {
00763     numContacts = num_contacts_allowable_contacts_test_;
00764   }
00765   //this just broadcasts collisions for the most recent collision space calls
00766   if(onCollisionContact_) {
00767     std::vector<collision_space::EnvironmentModel::Contact> contacts;
00768     getEnvironmentModel()->getCollisionContacts(allowedContacts_, contacts, numContacts); 
00769     ROS_DEBUG("Callback defined with %u contacts", (unsigned int) contacts.size());
00770     for (unsigned int i = 0 ; i < contacts.size() ; ++i) {
00771       onCollisionContact_(contacts[i]);
00772     } 
00773   } 
00774   else 
00775   {
00776     return false;
00777   }
00778   return true;
00779 }
00780 
00781 //TODO - write this function
00782 // bool planning_environment::PlanningMonitor::isRobotTrajectoryValid(const trajectory_msgs::JointTrajectory &trajectory, 
00783 //                                                                    motion_planning_msgs::RobotState &robot_state, 
00784 //                                                                    unsigned int start, 
00785 //                                                                    unsigned int end, 
00786 //                                                                    const int test, 
00787 //                                                                    bool verbose,
00788 //                                                                    motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00789 //                                                                    std::vector<motion_planning_msgs::ArmNavigationErrorCodes> &trajectory_error_codes) const {
00790   
00791 // }
00792 
00793 
00794 void planning_environment::PlanningMonitor::setAllowedContacts(const std::vector<collision_space::EnvironmentModel::AllowedContact> &allowedContacts)
00795 {
00796   allowedContacts_ = allowedContacts;
00797 }
00798 
00799 void planning_environment::PlanningMonitor::setAllowedContacts(const std::vector<motion_planning_msgs::AllowedContactSpecification> &allowed_contacts)
00800 {  
00801   allowedContacts_.clear();
00802   for (unsigned int i = 0 ; i < allowed_contacts.size() ; ++i)
00803   {
00804     collision_space::EnvironmentModel::AllowedContact ac;
00805     if (computeAllowedContact(allowed_contacts[i], ac))
00806       allowedContacts_.push_back(ac);
00807   }
00808 }
00809 
00810 const std::vector<collision_space::EnvironmentModel::AllowedContact>& planning_environment::PlanningMonitor::getAllowedContacts(void) const
00811 {
00812   return allowedContacts_;
00813 }
00814 
00815 void planning_environment::PlanningMonitor::printAllowedContacts(std::ostream &out)
00816 {
00817   out << allowedContacts_.size() << " allowed contacts" << std::endl;
00818   for (unsigned int i = 0 ; i < allowedContacts_.size() ; ++i)
00819   {
00820     out << "  - allowing contacts up to depth " << allowedContacts_[i].depth << " between links: [";
00821     for (unsigned int j = 0 ; j < allowedContacts_[i].links.size() ; ++j)
00822       out << allowedContacts_[i].links[j];
00823     out << "] and bound " << allowedContacts_[i].bound.get() << std::endl;
00824   }
00825 }
00826 
00827 void planning_environment::PlanningMonitor::printConstraints(std::ostream &out)
00828 {
00829   out << "Path constraints:" << std::endl;
00830 
00831   KinematicConstraintEvaluatorSet constraint_evaluator;
00832   constraint_evaluator.add(path_constraints_.joint_constraints);
00833   constraint_evaluator.add(path_constraints_.position_constraints);
00834   constraint_evaluator.add(path_constraints_.orientation_constraints);
00835   constraint_evaluator.add(path_constraints_.visibility_constraints);
00836   constraint_evaluator.print(out);
00837 
00838   out << "Goal constraints:" << std::endl;
00839   constraint_evaluator.clear();
00840   constraint_evaluator.add(goal_constraints_.joint_constraints);
00841   constraint_evaluator.add(goal_constraints_.position_constraints);
00842   constraint_evaluator.add(goal_constraints_.orientation_constraints);
00843   constraint_evaluator.add(goal_constraints_.visibility_constraints);
00844   constraint_evaluator.print(out);
00845 }
00846 
00847 void planning_environment::PlanningMonitor::clearAllowedContacts(void)
00848 {
00849   allowedContacts_.clear();
00850 }
00851 
00852 void planning_environment::PlanningMonitor::setCollisionCheck(const std::string link_name, bool state)
00853 {
00854   getEnvironmentModel()->setCollisionCheck(link_name,state);
00855 }
00856 
00857 void planning_environment::PlanningMonitor::setCollisionCheckAll(bool state)
00858 {
00859   getEnvironmentModel()->setCollisionCheckAll(state);
00860 }
00861 
00862 void planning_environment::PlanningMonitor::setCollisionCheckLinks(const std::vector<std::string> &link_names, bool state)
00863 {
00864   getEnvironmentModel()->setCollisionCheckLinks(link_names,state);
00865 }
00866 
00867 void planning_environment::PlanningMonitor::setCollisionCheckOnlyLinks(const std::vector<std::string> &link_names, bool state)
00868 {
00869   getEnvironmentModel()->setCollisionCheckOnlyLinks(link_names,state);
00870 }
00871 
00872 void planning_environment::PlanningMonitor::getChildLinks(const std::vector<std::string> &joints, 
00873                                                           std::vector<std::string> &link_names)
00874 {
00875   std::set<std::string> links_set;
00876 
00877   for(unsigned int i=0; i < joints.size(); i++)
00878   {
00879     const planning_models::KinematicModel::JointModel *joint = getKinematicModel()->getJointModel(joints[i]);
00880     if(joint)
00881     {
00882       if(joint->getChildLinkModel()) {
00883         std::vector<const planning_models::KinematicModel::LinkModel*> child_links;
00884         getKinematicModel()->getChildLinkModels(joint->getChildLinkModel(), child_links);
00885         for(std::vector<const planning_models::KinematicModel::LinkModel*>::iterator it = child_links.begin();
00886             it != child_links.end();
00887             it++) {
00888           links_set.insert((*it)->getName());
00889         }
00890       }
00891     }
00892   }
00893   for(std::set<std::string>::iterator set_iterator = links_set.begin(); set_iterator!= links_set.end(); set_iterator++)
00894   {
00895     link_names.push_back(*set_iterator);
00896   }
00897 }
00898 
00899 void planning_environment::PlanningMonitor::getOrderedCollisionOperationsForOnlyCollideLinks(const std::vector<std::string> &collision_check_links, 
00900                                                                                              const motion_planning_msgs::OrderedCollisionOperations &requested_collision_operations,
00901                                                                                              motion_planning_msgs::OrderedCollisionOperations &result_collision_operations)
00902 {
00903   motion_planning_msgs::OrderedCollisionOperations result;
00904   motion_planning_msgs::CollisionOperation op;
00905   std::vector<motion_planning_msgs::CollisionOperation> self_collisions;
00906 
00907   //this disables everything and everything
00908   op.object1 = op.COLLISION_SET_ALL;
00909   op.object2 = op.COLLISION_SET_ALL;
00910   op.operation = op.operation = motion_planning_msgs::CollisionOperation::DISABLE;;
00911   result.collision_operations.push_back(op);
00912   
00913   //now we need to add bodies attached to these links
00914   std::vector<std::string> all_collision_links = collision_check_links;
00915   const std::vector<const planning_models::KinematicModel::AttachedBodyModel*> att_vec = getEnvironmentModel()->getAttachedBodies();
00916   for(unsigned int i = 0; i < att_vec.size(); i++) 
00917   {
00918     for(std::vector<std::string>::const_iterator it = collision_check_links.begin();
00919         it != collision_check_links.end();
00920         it++) 
00921     {
00922       if(att_vec[i]->getAttachedLinkModel()->getName() == (*it)) {
00923         all_collision_links.push_back(att_vec[i]->getName());
00924       }
00925     }
00926   }
00927 
00928   //this enables collision_check_links with everything
00929   for(std::vector<std::string>::const_iterator it = all_collision_links.begin();
00930       it != all_collision_links.end();
00931       it++) 
00932   {
00933     op.object1 = (*it);
00934     op.object2 = op.COLLISION_SET_ALL;
00935     op.operation = motion_planning_msgs::CollisionOperation::ENABLE;
00936     result.collision_operations.push_back(op);
00937   } 
00938 
00939   std::vector<std::vector<bool> > current_allowed;
00940   std::map<std::string, unsigned int> vec_indices;
00941   //this disables collision_check_links with things they are allowed to collide with
00942   getEnvironmentModel()->getDefaultAllowedCollisionMatrix(current_allowed, vec_indices);
00943   for(std::vector<std::string>::const_iterator it = all_collision_links.begin();
00944       it != all_collision_links.end();it++)
00945   {
00946     std::map<std::string, unsigned int>::iterator map_it = vec_indices.find(*it);
00947     if(map_it == vec_indices.end())
00948       continue;
00949     unsigned int index = map_it->second;
00950     for(std::map<std::string, unsigned int>::iterator index_it = vec_indices.begin(); index_it != vec_indices.end(); index_it++)
00951     {
00952       if(current_allowed[index][index_it->second])
00953       { 
00954         op.object1 = (*it);
00955         op.object2 = index_it->first;
00956         op.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00957         result.collision_operations.push_back(op);    
00958       }
00959     }
00960   }  
00961 
00962   //this adds extra requested collision operations
00963   for(std::vector<motion_planning_msgs::CollisionOperation>::const_iterator it = requested_collision_operations.collision_operations.begin();
00964       it != requested_collision_operations.collision_operations.end();it++)
00965   {
00966     result.collision_operations.push_back(*it);
00967   }  
00968 
00969   result_collision_operations.collision_operations = result.collision_operations;
00970 
00971 }
00972 
00973 bool planning_environment::PlanningMonitor::checkPathConstraints(const planning_models::KinematicState* state, bool verbose) const {
00974   KinematicConstraintEvaluatorSet constraint_evaluator;
00975 
00976   constraint_evaluator.add(path_constraints_.joint_constraints);
00977   constraint_evaluator.add(path_constraints_.position_constraints);
00978   constraint_evaluator.add(path_constraints_.orientation_constraints);
00979   constraint_evaluator.add(path_constraints_.visibility_constraints);
00980   return(constraint_evaluator.decide(state, verbose));
00981 }
00982 
00983 bool planning_environment::PlanningMonitor::checkGoalConstraints(const planning_models::KinematicState* state, bool verbose) const {
00984   KinematicConstraintEvaluatorSet constraint_evaluator;
00985 
00986   constraint_evaluator.add(goal_constraints_.joint_constraints);
00987   constraint_evaluator.add(goal_constraints_.position_constraints);
00988   constraint_evaluator.add(goal_constraints_.orientation_constraints);
00989   constraint_evaluator.add(goal_constraints_.visibility_constraints);
00990   return(constraint_evaluator.decide(state, verbose));
00991 }