PlaceSequenceValidator.cpp
Go to the documentation of this file.
00001 /*
00002  * PlaceSequenceValidator.cpp
00003  *
00004  *  Created on: Oct 10, 2012
00005  */
00006 
00007 /*
00008  * Software License Agreement (BSD License)
00009  *
00010  * Copyright (c) 2011, Southwest Research Institute
00011  * All rights reserved.
00012  *
00013  * Redistribution and use in source and binary forms, with or without
00014  * modification, are permitted provided that the following conditions are met:
00015  *
00016  *       * Redistributions of source code must retain the above copyright
00017  *       notice, this list of conditions and the following disclaimer.
00018  *       * Redistributions in binary form must reproduce the above copyright
00019  *       notice, this list of conditions and the following disclaimer in the
00020  *       documentation and/or other materials provided with the distribution.
00021  *       * Neither the name of the Southwest Research Institute, nor the names
00022  *       of its contributors may be used to endorse or promote products derived
00023  *       from this software without specific prior written permission.
00024  *
00025  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00026  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00027  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00028  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00029  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00030  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00031  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00032  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00033  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00034  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  * POSSIBILITY OF SUCH DAMAGE.
00036  */
00037 
00038 #include <object_manipulation_tools/manipulation_utils/PlaceSequenceValidator.h>
00039 #include <tf/transform_listener.h>
00040 using namespace object_manipulator;
00041 using namespace object_manipulation_msgs;
00042 
00043 PlaceSequenceValidator::PlaceSequenceValidator()
00044 {
00045 
00046 }
00047 
00048 PlaceSequenceValidator::PlaceSequenceValidator(planning_environment::CollisionModels* cm,
00049                   const std::string& plugin_name)
00050 :object_manipulator::PlaceTesterFast(cm,plugin_name),
00051  _TcpToWristTransform(tf::Transform::getIdentity())
00052 {
00053         // TODO Auto-generated constructor stub
00054 
00055 }
00056 
00057 PlaceSequenceValidator::~PlaceSequenceValidator() {
00058         // TODO Auto-generated destructor stub
00059 }
00060 
00061 void PlaceSequenceValidator::setTcpToWristTransform(const tf::Transform &tcpTransform)
00062 {
00063         _TcpToWristTransform = tcpTransform;
00064 }
00065 
00066 void PlaceSequenceValidator::testPlaces(const object_manipulation_msgs::PlaceGoal &place_goal,
00067         const std::vector<geometry_msgs::PoseStamped> &place_locations,
00068         std::vector<object_manipulator::PlaceExecutionInfo> &execution_info,
00069         bool return_on_first_hit)
00070 {
00071   ros::WallTime start = ros::WallTime::now();
00072 
00073   std::map<unsigned int, unsigned int> outcome_count;
00074 
00075   planning_environment::CollisionModels* cm = getCollisionModels();
00076   planning_models::KinematicState* state = getPlanningSceneState();
00077 
00078   std::map<std::string, double> planning_scene_state_values;
00079   state->getKinematicStateValues(planning_scene_state_values);
00080 
00081   std::vector<std::string> end_effector_links, arm_links;
00082   getGroupLinks(handDescription().gripperCollisionName(place_goal.arm_name), end_effector_links);
00083   getGroupLinks(handDescription().armGroup(place_goal.arm_name), arm_links);
00084 
00085   collision_space::EnvironmentModel::AllowedCollisionMatrix original_acm = cm->getCurrentAllowedCollisionMatrix();
00086   cm->disableCollisionsForNonUpdatedLinks(place_goal.arm_name);
00087   collision_space::EnvironmentModel::AllowedCollisionMatrix group_disable_acm = cm->getCurrentAllowedCollisionMatrix();
00088   collision_space::EnvironmentModel::AllowedCollisionMatrix object_disable_acm = group_disable_acm;
00089   if(!place_goal.collision_support_surface_name.empty()) {
00090     if(place_goal.collision_support_surface_name == "\"all\"")
00091     {
00092       object_disable_acm.changeEntry(place_goal.collision_object_name, true);
00093     }
00094     else
00095     {
00096       object_disable_acm.changeEntry(place_goal.collision_object_name, place_goal.collision_support_surface_name, true);
00097     }
00098   }
00099   collision_space::EnvironmentModel::AllowedCollisionMatrix object_support_disable_acm = object_disable_acm;
00100   if(place_goal.allow_gripper_support_collision) {
00101     if(place_goal.collision_support_surface_name == "\"all\"")
00102     {
00103       for(unsigned int i = 0; i < end_effector_links.size(); i++)
00104       {
00105           object_support_disable_acm.changeEntry(end_effector_links[i], true);
00106       }
00107     }
00108     else
00109     {
00110       object_support_disable_acm.changeEntry(place_goal.collision_support_surface_name, end_effector_links, true);
00111     }
00112   }
00113   collision_space::EnvironmentModel::AllowedCollisionMatrix object_all_arm_disable_acm = object_disable_acm;
00114   collision_space::EnvironmentModel::AllowedCollisionMatrix object_support_all_arm_disable_acm = object_support_disable_acm;
00115   collision_space::EnvironmentModel::AllowedCollisionMatrix group_all_arm_disable_acm = group_disable_acm;
00116 
00117   //turning off collisions for the arm associated with this end effector
00118   for(unsigned int i = 0; i < arm_links.size(); i++) {
00119     object_all_arm_disable_acm.changeEntry(arm_links[i], true);
00120     object_support_all_arm_disable_acm.changeEntry(arm_links[i], true);
00121     group_all_arm_disable_acm.changeEntry(arm_links[i], true);
00122   }
00123   cm->setAlteredAllowedCollisionMatrix(object_support_all_arm_disable_acm);
00124 
00125   //first we apply link padding for place check
00126   cm->applyLinkPaddingToCollisionSpace(linkPaddingForPlace(place_goal));
00127 
00128   execution_info.clear();
00129   execution_info.resize(place_locations.size());
00130 
00131   tf::Vector3 approach_dir;
00132   tf::vector3MsgToTF(doNegate(place_goal.approach.direction.vector), approach_dir);
00133   approach_dir.normalize();
00134   tf::Vector3 distance_approach_dir = approach_dir*fabs(place_goal.approach.desired_distance);
00135   tf::Transform approach_trans(tf::Quaternion(0,0,0,1.0), distance_approach_dir);
00136 
00137   tf::Vector3 retreat_dir;
00138   tf::vector3MsgToTF(doNegate(handDescription().approachDirection(place_goal.arm_name)), retreat_dir);
00139   retreat_dir.normalize();
00140   tf::Vector3 distance_retreat_dir = retreat_dir*fabs(place_goal.desired_retreat_distance);
00141   tf::Transform retreat_trans(tf::Quaternion(0,0,0,1.0), distance_retreat_dir);
00142 
00143   std::map<std::string, double> planning_scene_state_values_post_grasp = planning_scene_state_values_post_grasp;
00144   std::map<std::string, double> post_grasp_joint_vals;
00145   std::map<std::string, double> grasp_joint_vals;
00146   for(unsigned int j = 0; j < place_goal.grasp.pre_grasp_posture.name.size(); j++)
00147   {
00148     planning_scene_state_values_post_grasp[place_goal.grasp.pre_grasp_posture.name[j]] = place_goal.grasp.pre_grasp_posture.position[j];
00149     post_grasp_joint_vals[place_goal.grasp.pre_grasp_posture.name[j]] = place_goal.grasp.pre_grasp_posture.position[j];
00150     grasp_joint_vals[place_goal.grasp.pre_grasp_posture.name[j]] = planning_scene_state_values[place_goal.grasp.pre_grasp_posture.name[j]];
00151   }
00152 
00153   // store all gripper poses relative to the world
00154   std::vector<tf::Transform> gripperTcpInWorldPoses(place_locations.size());
00155 
00156   //now this is place specific
00157   for(unsigned int i = 0; i < place_locations.size(); i++)
00158   {
00159     //using the grasp posture
00160     state->setKinematicState(post_grasp_joint_vals);
00161 
00162     //always true
00163     execution_info[i].result_.continuation_possible = true;
00164 
00165     if(!cm->convertPoseGivenWorldTransform(*state,
00166                                            cm->getWorldFrameId(),
00167                                            place_locations[i].header,
00168                                            place_locations[i].pose,
00169                                            execution_info[i].gripper_place_pose_))
00170     {
00171       ROS_INFO_STREAM("Something wrong with pose conversion");
00172       continue;
00173     }
00174     tf::poseMsgToTF(execution_info[i].gripper_place_pose_.pose, gripperTcpInWorldPoses[i]);
00175     tf::Transform gripperInObjPose; // wrist frame relative to object
00176     tf::poseMsgToTF(place_goal.grasp.grasp_pose, gripperInObjPose);
00177 
00178     //post multiply for object frame in order to obtain gripper pose in world coordinates
00179     gripperTcpInWorldPoses[i] = gripperTcpInWorldPoses[i]*gripperInObjPose;
00180 
00181 
00182     tf::poseTFToMsg(gripperTcpInWorldPoses[i], execution_info[i].gripper_place_pose_.pose);
00183     state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(place_goal.arm_name),gripperTcpInWorldPoses[i]);
00184 
00185     if(cm->isKinematicStateInCollision(*state))
00186     {
00187       ROS_INFO_STREAM("gripper at place move is in collision");
00188       execution_info[i].result_.result_code = PlaceLocationResult::PLACE_IN_COLLISION;
00189       outcome_count[PlaceLocationResult::PLACE_IN_COLLISION]++;
00190     }
00191     else
00192     {
00193         //ROS_INFO_STREAM("gripper at place move is collision free");
00194         execution_info[i].result_.result_code = 0;
00195     }
00196   }
00197 
00198   //now we revert link paddings for approach and retreat checks
00199   cm->revertCollisionSpacePaddingToDefault();
00200 
00201   //now we do the place approach pose, not allowing anything different
00202   cm->setAlteredAllowedCollisionMatrix(group_all_arm_disable_acm);
00203 
00204   for(unsigned int i = 0; i < place_locations.size(); i++)
00205   {
00206 
00207     if(execution_info[i].result_.result_code != 0) continue;
00208 
00209     state->setKinematicState(planning_scene_state_values);
00210 
00211     tf::Transform approach_pose = approach_trans*gripperTcpInWorldPoses[i];
00212     state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(place_goal.arm_name),approach_pose);
00213 
00214     if(cm->isKinematicStateInCollision(*state))
00215     {
00216       ROS_INFO_STREAM("gripper at pre-place is in collision");
00217 
00218       execution_info[i].result_.result_code = PlaceLocationResult::PREPLACE_IN_COLLISION;
00219       outcome_count[PlaceLocationResult::PREPLACE_IN_COLLISION]++;
00220       continue;
00221     }
00222     else
00223     {
00224         //ROS_INFO_STREAM("gripper at pre-place move is collision free");
00225     }
00226   }
00227 
00228   //TODO - for now, just have to hope that we're not in contact with the object
00229   //after we release, but there's not a good way to check this for now
00230   //so we just leave the object all arm disable on for the retreat position check
00231   cm->setAlteredAllowedCollisionMatrix(object_all_arm_disable_acm);
00232 
00233   //first we do retreat, with the gripper at post grasp position
00234   for(unsigned int i = 0; i < place_locations.size(); i++) {
00235 
00236     if(execution_info[i].result_.result_code != 0) continue;
00237 
00238     state->setKinematicState(post_grasp_joint_vals);
00239 
00240     tf::Transform retreat_pose = gripperTcpInWorldPoses[i]*retreat_trans;
00241     state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(place_goal.arm_name),retreat_pose);
00242 
00243     if(cm->isKinematicStateInCollision(*state))
00244     {
00245       /*
00246         std_msgs::ColorRGBA col_pregrasp;
00247         col_pregrasp.r = 0.0;
00248         col_pregrasp.g = 1.0;
00249         col_pregrasp.b = 1.0;
00250         col_pregrasp.a = 1.0;
00251         visualization_msgs::MarkerArray arr;
00252         cm_->getRobotMarkersGivenState(*state, arr, col_pregrasp,
00253                                        "retreat",
00254                                        ros::Duration(0.0),
00255                                        &end_effector_links);
00256         vis_marker_array_publisher_.publish(arr);
00257       */
00258 
00259       ROS_INFO_STREAM("gripper at retreat move is in collision");
00260       execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_IN_COLLISION;
00261       outcome_count[PlaceLocationResult::RETREAT_IN_COLLISION]++;
00262 
00263     }
00264     else
00265     {
00266         //ROS_INFO_STREAM("gripper at retreat move is collision free");
00267     }
00268   }
00269 
00270   std_msgs::Header world_header;
00271   world_header.frame_id = cm->getWorldFrameId();
00272   const std::vector<std::string>& joint_names = ik_solver_map_[place_goal.arm_name]->getJointNames();
00273 
00274   if(return_on_first_hit)
00275   {
00276 
00277     bool last_ik_failed = false;
00278     for(unsigned int i = 0; i < place_locations.size(); i++)
00279     {
00280 
00281       if(execution_info[i].result_.result_code != 0) continue;
00282 
00283       if(!last_ik_failed)
00284       {
00285         //now we move to the ik portion, which requires re-enabling collisions for the arms
00286         cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00287 
00288         //and also reducing link paddings
00289         cm->applyLinkPaddingToCollisionSpace(linkPaddingForPlace(place_goal));
00290       }
00291       //getting back to original state for seed
00292       state->setKinematicState(planning_scene_state_values_post_grasp);
00293 
00294       //now call ik for grasp
00295       geometry_msgs::Pose place_geom_pose; // pose of the wrist relative to the world
00296       tf::poseTFToMsg(gripperTcpInWorldPoses[i]*_TcpToWristTransform, place_geom_pose);
00297 
00298       geometry_msgs::PoseStamped wristInArmBasePose; // pose of the wrist relative to the arm base
00299       cm->convertPoseGivenWorldTransform(*state,
00300                                          ik_solver_map_[place_goal.arm_name]->getBaseName(),
00301                                          world_header,
00302                                          place_geom_pose,
00303                                          wristInArmBasePose);
00304 
00305       arm_navigation_msgs::Constraints emp;
00306       sensor_msgs::JointState solution;
00307       arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00308       if(!ik_solver_map_[place_goal.arm_name]->findConstraintAwareSolution(wristInArmBasePose.pose,
00309                                                                            emp,
00310                                                                            state,
00311                                                                            solution,
00312                                                                            error_code,
00313                                                                            false))
00314       {
00315         ROS_INFO_STREAM("Place out of reach, skipping to next candidate");
00316         execution_info[i].result_.result_code = PlaceLocationResult::PLACE_OUT_OF_REACH;
00317         outcome_count[PlaceLocationResult::PLACE_OUT_OF_REACH]++;
00318         last_ik_failed = true;
00319         continue;
00320       }
00321       else
00322       {
00323         ROS_INFO_STREAM("Place move is in reach");
00324         last_ik_failed = false;
00325       }
00326 
00327       state->setKinematicState(grasp_joint_vals);
00328 
00329       //now we solve interpolated ik
00330       tf::Transform wristInArmBaseTransform;
00331       tf::poseMsgToTF(wristInArmBasePose.pose, wristInArmBaseTransform);
00332       //now we need to do interpolated ik
00333       execution_info[i].descend_trajectory_.joint_names = joint_names;
00334 
00335       if(!getInterpolatedIK(place_goal.arm_name,
00336                             wristInArmBaseTransform,
00337                             approach_dir,
00338                             place_goal.approach.desired_distance,
00339                             solution.position,
00340                             true,
00341                             true,
00342                             execution_info[i].descend_trajectory_))
00343       {
00344         ROS_INFO_STREAM("No interpolated IK for approach to place");
00345         execution_info[i].result_.result_code = PlaceLocationResult::PLACE_UNFEASIBLE;
00346         outcome_count[PlaceLocationResult::PLACE_UNFEASIBLE]++;
00347         continue;
00348       }
00349       else
00350       {
00351           ROS_INFO_STREAM("Interpolated IK for approach move found");
00352       }
00353 
00354       state->setKinematicState(planning_scene_state_values_post_grasp);
00355       execution_info[i].retreat_trajectory_.joint_names = joint_names;
00356       if(!getInterpolatedIK(place_goal.arm_name,
00357                             wristInArmBaseTransform,
00358                             retreat_dir,
00359                             place_goal.desired_retreat_distance,
00360                             solution.position,
00361                             false,
00362                             true,
00363                             execution_info[i].retreat_trajectory_))
00364       {
00365         ROS_INFO_STREAM("No interpolated IK for place to retreat");
00366         execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_UNFEASIBLE;
00367         outcome_count[PlaceLocationResult::RETREAT_UNFEASIBLE]++;
00368         continue;
00369       }
00370       else
00371       {
00372           ROS_INFO_STREAM("Interpolated IK for place move found");
00373       }
00374 
00375       //now we revert link paddings and object collisions and do a final check for the initial ik points
00376       cm->revertCollisionSpacePaddingToDefault();
00377 
00378       //the start of the place approach needs to be collision-free according to the default collision matrix
00379       cm->setAlteredAllowedCollisionMatrix(group_disable_acm);
00380 
00381       if(execution_info[i].descend_trajectory_.points.empty()) {
00382         ROS_WARN_STREAM("No result code and no points in approach trajectory");
00383         continue;
00384       }
00385 
00386       std::map<std::string, double> pre_place_ik = grasp_joint_vals;
00387       for(unsigned int j = 0; j < joint_names.size(); j++)
00388       {
00389         pre_place_ik[joint_names[j]] = execution_info[i].descend_trajectory_.points[0].positions[j];
00390       }
00391 
00392 
00393       state->setKinematicState(pre_place_ik);
00394       if(cm->isKinematicStateInCollision(*state))
00395       {
00396         ROS_INFO_STREAM("Final collision pre-place check failed");
00397         execution_info[i].result_.result_code = PlaceLocationResult::PREPLACE_OUT_OF_REACH;
00398         outcome_count[PlaceLocationResult::PREPLACE_OUT_OF_REACH]++;
00399         continue;
00400       }
00401 
00402       //the end of the place retreat also needs to be collision-free according to the default collision matrix
00403       if(execution_info[i].retreat_trajectory_.points.empty())
00404       {
00405         ROS_WARN_STREAM("No result code and no points in retreat trajectory");
00406         continue;
00407       }
00408 
00409       std::map<std::string, double> retreat_ik = post_grasp_joint_vals;
00410       for(unsigned int j = 0; j < joint_names.size(); j++)
00411       {
00412        retreat_ik[joint_names[j]] = execution_info[i].retreat_trajectory_.points.back().positions[j];
00413       }
00414       state->setKinematicState(retreat_ik);
00415       if(cm->isKinematicStateInCollision(*state))
00416       {
00417         ROS_INFO_STREAM("Final collision retreat check failed");
00418         execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_OUT_OF_REACH;
00419         outcome_count[PlaceLocationResult::RETREAT_OUT_OF_REACH]++;
00420         continue;
00421       }
00422       else
00423       {
00424         ROS_INFO_STREAM("Everything successful");
00425         execution_info[i].result_.result_code = PlaceLocationResult::SUCCESS;
00426         execution_info.resize(i+1);
00427         outcome_count[PlaceLocationResult::SUCCESS]++;
00428         break;
00429       }
00430     }
00431 
00432     for(std::map<unsigned int, unsigned int>::iterator it = outcome_count.begin();
00433         it != outcome_count.end();
00434         it++)
00435     {
00436       ROS_INFO_STREAM("Outcome " << it->first << " count " << it->second);
00437     }
00438     cm->setAlteredAllowedCollisionMatrix(original_acm);
00439     return;
00440   }
00441 
00442   //now we move to the ik portion, which requires re-enabling collisions for the arms
00443   cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00444 
00445   //and also reducing link paddings
00446   cm->applyLinkPaddingToCollisionSpace(linkPaddingForPlace(place_goal));
00447 
00448   for(unsigned int i = 0; i < place_locations.size(); i++)
00449   {
00450 
00451     if(execution_info[i].result_.result_code != 0) continue;
00452 
00453     //getting back to original state for seed
00454     state->setKinematicState(planning_scene_state_values_post_grasp);
00455 
00456     //now call ik for grasp
00457     geometry_msgs::Pose place_geom_pose;
00458     tf::poseTFToMsg(gripperTcpInWorldPoses[i]*_TcpToWristTransform, place_geom_pose);
00459     geometry_msgs::PoseStamped wristInArmBasePose;
00460     cm->convertPoseGivenWorldTransform(*state,
00461                                        ik_solver_map_[place_goal.arm_name]->getBaseName(),
00462                                        world_header,
00463                                        place_geom_pose,
00464                                        wristInArmBasePose);
00465 
00466     arm_navigation_msgs::Constraints emp;
00467     sensor_msgs::JointState solution;
00468     arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00469     if(!ik_solver_map_[place_goal.arm_name]->findConstraintAwareSolution(wristInArmBasePose.pose,
00470                                                                          emp,
00471                                                                          state,
00472                                                                          solution,
00473                                                                          error_code,
00474                                                                          false))
00475     {
00476       ROS_INFO_STREAM("Place out of reach");
00477       execution_info[i].result_.result_code = PlaceLocationResult::PLACE_OUT_OF_REACH;
00478       outcome_count[PlaceLocationResult::PLACE_OUT_OF_REACH]++;
00479       continue;
00480     }
00481 
00482     state->setKinematicState(grasp_joint_vals);
00483 
00484     //now we solve interpolated ik
00485     tf::Transform wristInArmBaseTransform;
00486     tf::poseMsgToTF(wristInArmBasePose.pose, wristInArmBaseTransform);
00487     //now we need to do interpolated ik
00488     execution_info[i].descend_trajectory_.joint_names = joint_names;
00489     if(!getInterpolatedIK(place_goal.arm_name,
00490                           wristInArmBaseTransform,
00491                           approach_dir,
00492                           place_goal.approach.desired_distance,
00493                           solution.position,
00494                           true,
00495                           true,
00496                           execution_info[i].descend_trajectory_)) {
00497       ROS_INFO_STREAM("No interpolated IK for approach to place");
00498       execution_info[i].result_.result_code = PlaceLocationResult::PLACE_UNFEASIBLE;
00499       outcome_count[PlaceLocationResult::PLACE_UNFEASIBLE]++;
00500       continue;
00501     }
00502 
00503     state->setKinematicState(planning_scene_state_values_post_grasp);
00504     execution_info[i].retreat_trajectory_.joint_names = joint_names;
00505     if(!getInterpolatedIK(place_goal.arm_name,
00506                           wristInArmBaseTransform,
00507                           retreat_dir,
00508                           place_goal.desired_retreat_distance,
00509                           solution.position,
00510                           false,
00511                           false,
00512                           execution_info[i].retreat_trajectory_))
00513     {
00514       ROS_INFO_STREAM("No interpolated IK for place to retreat");
00515       execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_UNFEASIBLE;
00516       outcome_count[PlaceLocationResult::RETREAT_UNFEASIBLE]++;
00517       continue;
00518     }
00519   }
00520 
00521   //now we revert link paddings and object collisions and do a final check for the initial ik points
00522   cm->revertCollisionSpacePaddingToDefault();
00523 
00524   cm->setAlteredAllowedCollisionMatrix(group_disable_acm);
00525 
00526   for(unsigned int i = 0; i < place_locations.size(); i++)
00527   {
00528 
00529     if(execution_info[i].result_.result_code != 0) continue;
00530 
00531     if(execution_info[i].descend_trajectory_.points.empty())
00532     {
00533       ROS_WARN_STREAM("No result code and no points in approach trajectory");
00534       continue;
00535     }
00536 
00537     std::map<std::string, double> pre_place_ik = grasp_joint_vals;
00538     for(unsigned int j = 0; j < joint_names.size(); j++)
00539     {
00540       pre_place_ik[joint_names[j]] = execution_info[i].descend_trajectory_.points[0].positions[j];
00541     }
00542 
00543     state->setKinematicState(pre_place_ik);
00544     if(cm->isKinematicStateInCollision(*state))
00545     {
00546       ROS_INFO_STREAM("Final pre-place check failed");
00547       execution_info[i].result_.result_code = PlaceLocationResult::PREPLACE_OUT_OF_REACH;
00548       outcome_count[PlaceLocationResult::PREPLACE_OUT_OF_REACH]++;
00549       continue;
00550     }
00551 
00552   }
00553 
00554   //now we need to disable collisions with the object for lift
00555   cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00556 
00557   for(unsigned int i = 0; i < place_locations.size(); i++)
00558   {
00559 
00560     if(execution_info[i].result_.result_code != 0) continue;
00561 
00562     if(execution_info[i].retreat_trajectory_.points.empty())
00563     {
00564       ROS_WARN_STREAM("No result code and no points in retreat trajectory");
00565       continue;
00566     }
00567 
00568     std::map<std::string, double> retreat_ik = post_grasp_joint_vals;
00569     for(unsigned int j = 0; j < joint_names.size(); j++)
00570     {
00571       retreat_ik[joint_names[j]] = execution_info[i].retreat_trajectory_.points.back().positions[j];
00572     }
00573 
00574     state->setKinematicState(retreat_ik);
00575     if(cm->isKinematicStateInCollision(*state))
00576     {
00577       ROS_INFO_STREAM("Final lift check failed");
00578       execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_OUT_OF_REACH;
00579       outcome_count[PlaceLocationResult::RETREAT_OUT_OF_REACH]++;
00580       continue;
00581     }
00582     else
00583     {
00584       ROS_INFO_STREAM("Everything successful");
00585       execution_info[i].result_.result_code = PlaceLocationResult::SUCCESS;
00586       outcome_count[PlaceLocationResult::SUCCESS]++;
00587     }
00588   }
00589   cm->setAlteredAllowedCollisionMatrix(original_acm);
00590 
00591   ROS_INFO_STREAM("Took " << (ros::WallTime::now()-start).toSec());
00592 
00593   for(std::map<unsigned int, unsigned int>::iterator it = outcome_count.begin();
00594       it != outcome_count.end();
00595       it++) {
00596     ROS_INFO_STREAM("Outcome " << it->first << " count " << it->second);
00597   }
00598 }


object_manipulation_tools
Author(s): Jnicho
autogenerated on Mon Oct 6 2014 00:56:17