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


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04