00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
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());
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
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
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
00277 for(unsigned int i = 0; i < place_locations.size(); i++) {
00278
00279 state->setKinematicState(post_grasp_joint_vals);
00280
00281
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::poseTFToMsg(place_poses[i], execution_info[i].gripper_place_pose_.pose);
00294 state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(place_goal.arm_name),place_poses[i]);
00295
00296 if(cm->isKinematicStateInCollision(*state)) {
00297 ROS_DEBUG_STREAM("Place in collision");
00298 execution_info[i].result_.result_code = PlaceLocationResult::PLACE_IN_COLLISION;
00299 outcome_count[PlaceLocationResult::PLACE_IN_COLLISION]++;
00300 } else {
00301 execution_info[i].result_.result_code = 0;
00302 }
00303 }
00304
00305
00306 cm->revertCollisionSpacePaddingToDefault();
00307
00308
00309 cm->setAlteredAllowedCollisionMatrix(group_all_arm_disable_acm);
00310
00311 for(unsigned int i = 0; i < place_locations.size(); i++) {
00312
00313 if(execution_info[i].result_.result_code != 0) continue;
00314
00315 state->setKinematicState(planning_scene_state_values);
00316
00317 tf::Transform approach_pose = approach_trans*place_poses[i];
00318 state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(place_goal.arm_name),approach_pose);
00319
00320 if(cm->isKinematicStateInCollision(*state)) {
00321 ROS_DEBUG_STREAM("Preplace in collision");
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331 execution_info[i].result_.result_code = PlaceLocationResult::PREPLACE_IN_COLLISION;
00332 outcome_count[PlaceLocationResult::PREPLACE_IN_COLLISION]++;
00333 continue;
00334 }
00335 }
00336
00337
00338
00339
00340 cm->setAlteredAllowedCollisionMatrix(object_all_arm_disable_acm);
00341
00342
00343 for(unsigned int i = 0; i < place_locations.size(); i++) {
00344
00345 if(execution_info[i].result_.result_code != 0) continue;
00346
00347 state->setKinematicState(post_grasp_joint_vals);
00348
00349 tf::Transform retreat_pose = place_poses[i]*retreat_trans;
00350 state->updateKinematicStateWithLinkAt(handDescription().gripperFrame(place_goal.arm_name),retreat_pose);
00351
00352 if(cm->isKinematicStateInCollision(*state)) {
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366 ROS_DEBUG_STREAM("Retreat in collision");
00367 execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_IN_COLLISION;
00368 outcome_count[PlaceLocationResult::RETREAT_IN_COLLISION]++;
00369 }
00370 }
00371
00372 std_msgs::Header world_header;
00373 world_header.frame_id = cm->getWorldFrameId();
00374 const std::vector<std::string>& joint_names = ik_solver_map_[place_goal.arm_name]->getJointNames();
00375
00376 if(return_on_first_hit) {
00377
00378 bool last_ik_failed = false;
00379 for(unsigned int i = 0; i < place_locations.size(); i++) {
00380
00381 if(execution_info[i].result_.result_code != 0) continue;
00382
00383 if(!last_ik_failed) {
00384
00385 cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00386
00387
00388 cm->applyLinkPaddingToCollisionSpace(linkPaddingForPlace(place_goal));
00389 }
00390
00391 state->setKinematicState(planning_scene_state_values_post_grasp);
00392
00393
00394 geometry_msgs::Pose place_geom_pose;
00395 tf::poseTFToMsg(place_poses[i], place_geom_pose);
00396 geometry_msgs::PoseStamped base_link_place_pose;
00397 cm->convertPoseGivenWorldTransform(*state,
00398 ik_solver_map_[place_goal.arm_name]->getBaseName(),
00399 world_header,
00400 place_geom_pose,
00401 base_link_place_pose);
00402
00403 arm_navigation_msgs::Constraints emp;
00404 sensor_msgs::JointState solution;
00405 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00406 if(!ik_solver_map_[place_goal.arm_name]->findConstraintAwareSolution(base_link_place_pose.pose,
00407 emp,
00408 state,
00409 solution,
00410 error_code,
00411 false)) {
00412 ROS_DEBUG_STREAM("Place out of reach");
00413 execution_info[i].result_.result_code = PlaceLocationResult::PLACE_OUT_OF_REACH;
00414 outcome_count[PlaceLocationResult::PLACE_OUT_OF_REACH]++;
00415 last_ik_failed = true;
00416 continue;
00417 } else {
00418 last_ik_failed = false;
00419 }
00420
00421 state->setKinematicState(grasp_joint_vals);
00422
00423
00424 tf::Transform base_link_bullet_place_pose;
00425 tf::poseMsgToTF(base_link_place_pose.pose, base_link_bullet_place_pose);
00426
00427 execution_info[i].descend_trajectory_.joint_names = joint_names;
00428 if(!getInterpolatedIK(place_goal.arm_name,
00429 base_link_bullet_place_pose,
00430 approach_dir,
00431 place_goal.approach.desired_distance,
00432 solution.position,
00433 true,
00434 true,
00435 execution_info[i].descend_trajectory_)) {
00436 ROS_DEBUG_STREAM("No interpolated IK for approach to place");
00437 execution_info[i].result_.result_code = PlaceLocationResult::PLACE_UNFEASIBLE;
00438 outcome_count[PlaceLocationResult::PLACE_UNFEASIBLE]++;
00439 continue;
00440 }
00441
00442 state->setKinematicState(planning_scene_state_values_post_grasp);
00443 execution_info[i].retreat_trajectory_.joint_names = joint_names;
00444 if(!getInterpolatedIK(place_goal.arm_name,
00445 base_link_bullet_place_pose,
00446 retreat_dir,
00447 place_goal.desired_retreat_distance,
00448 solution.position,
00449 false,
00450 false,
00451 execution_info[i].retreat_trajectory_)) {
00452 ROS_DEBUG_STREAM("No interpolated IK for place to retreat");
00453 execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_UNFEASIBLE;
00454 outcome_count[PlaceLocationResult::RETREAT_UNFEASIBLE]++;
00455 continue;
00456 }
00457
00458
00459 cm->revertCollisionSpacePaddingToDefault();
00460
00461
00462 cm->setAlteredAllowedCollisionMatrix(group_disable_acm);
00463
00464 if(execution_info[i].descend_trajectory_.points.empty()) {
00465 ROS_WARN_STREAM("No result code and no points in approach trajectory");
00466 continue;
00467 }
00468
00469 std::map<std::string, double> pre_place_ik = grasp_joint_vals;
00470 for(unsigned int j = 0; j < joint_names.size(); j++) {
00471 pre_place_ik[joint_names[j]] = execution_info[i].descend_trajectory_.points[0].positions[j];
00472 }
00473 state->setKinematicState(pre_place_ik);
00474 if(cm->isKinematicStateInCollision(*state)) {
00475 ROS_DEBUG_STREAM("Final pre-place check failed");
00476 execution_info[i].result_.result_code = PlaceLocationResult::PREPLACE_OUT_OF_REACH;
00477 outcome_count[PlaceLocationResult::PREPLACE_OUT_OF_REACH]++;
00478 continue;
00479 }
00480
00481
00482 if(execution_info[i].retreat_trajectory_.points.empty()) {
00483 ROS_WARN_STREAM("No result code and no points in retreat trajectory");
00484 continue;
00485 }
00486 std::map<std::string, double> retreat_ik = post_grasp_joint_vals;
00487 for(unsigned int j = 0; j < joint_names.size(); j++) {
00488 retreat_ik[joint_names[j]] = execution_info[i].retreat_trajectory_.points.back().positions[j];
00489 }
00490 state->setKinematicState(retreat_ik);
00491 if(cm->isKinematicStateInCollision(*state)) {
00492 ROS_DEBUG_STREAM("Final retreat check failed");
00493 execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_OUT_OF_REACH;
00494 outcome_count[PlaceLocationResult::RETREAT_OUT_OF_REACH]++;
00495 continue;
00496 } else {
00497 ROS_INFO_STREAM("Everything successful");
00498 execution_info[i].result_.result_code = PlaceLocationResult::SUCCESS;
00499 execution_info.resize(i+1);
00500 outcome_count[PlaceLocationResult::SUCCESS]++;
00501 break;
00502 }
00503 }
00504 for(std::map<unsigned int, unsigned int>::iterator it = outcome_count.begin();
00505 it != outcome_count.end();
00506 it++) {
00507 ROS_INFO_STREAM("Outcome " << it->first << " count " << it->second);
00508 }
00509 cm->setAlteredAllowedCollisionMatrix(original_acm);
00510 return;
00511 }
00512
00513
00514 cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00515
00516
00517 cm->applyLinkPaddingToCollisionSpace(linkPaddingForPlace(place_goal));
00518
00519 for(unsigned int i = 0; i < place_locations.size(); i++) {
00520
00521 if(execution_info[i].result_.result_code != 0) continue;
00522
00523
00524 state->setKinematicState(planning_scene_state_values_post_grasp);
00525
00526
00527 geometry_msgs::Pose place_geom_pose;
00528 tf::poseTFToMsg(place_poses[i], place_geom_pose);
00529 geometry_msgs::PoseStamped base_link_place_pose;
00530 cm->convertPoseGivenWorldTransform(*state,
00531 ik_solver_map_[place_goal.arm_name]->getBaseName(),
00532 world_header,
00533 place_geom_pose,
00534 base_link_place_pose);
00535
00536 arm_navigation_msgs::Constraints emp;
00537 sensor_msgs::JointState solution;
00538 arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00539 if(!ik_solver_map_[place_goal.arm_name]->findConstraintAwareSolution(base_link_place_pose.pose,
00540 emp,
00541 state,
00542 solution,
00543 error_code,
00544 false)) {
00545 ROS_DEBUG_STREAM("Place out of reach");
00546 execution_info[i].result_.result_code = PlaceLocationResult::PLACE_OUT_OF_REACH;
00547 outcome_count[PlaceLocationResult::PLACE_OUT_OF_REACH]++;
00548 continue;
00549 }
00550
00551 state->setKinematicState(grasp_joint_vals);
00552
00553
00554 tf::Transform base_link_bullet_place_pose;
00555 tf::poseMsgToTF(base_link_place_pose.pose, base_link_bullet_place_pose);
00556
00557 execution_info[i].descend_trajectory_.joint_names = joint_names;
00558 if(!getInterpolatedIK(place_goal.arm_name,
00559 base_link_bullet_place_pose,
00560 approach_dir,
00561 place_goal.approach.desired_distance,
00562 solution.position,
00563 true,
00564 true,
00565 execution_info[i].descend_trajectory_)) {
00566 ROS_DEBUG_STREAM("No interpolated IK for approach to place");
00567 execution_info[i].result_.result_code = PlaceLocationResult::PLACE_UNFEASIBLE;
00568 outcome_count[PlaceLocationResult::PLACE_UNFEASIBLE]++;
00569 continue;
00570 }
00571
00572 state->setKinematicState(planning_scene_state_values_post_grasp);
00573 execution_info[i].retreat_trajectory_.joint_names = joint_names;
00574 if(!getInterpolatedIK(place_goal.arm_name,
00575 base_link_bullet_place_pose,
00576 retreat_dir,
00577 place_goal.desired_retreat_distance,
00578 solution.position,
00579 false,
00580 false,
00581 execution_info[i].retreat_trajectory_)) {
00582 ROS_DEBUG_STREAM("No interpolated IK for place to retreat");
00583 execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_UNFEASIBLE;
00584 outcome_count[PlaceLocationResult::RETREAT_UNFEASIBLE]++;
00585 continue;
00586 }
00587 }
00588
00589
00590 cm->revertCollisionSpacePaddingToDefault();
00591
00592 cm->setAlteredAllowedCollisionMatrix(group_disable_acm);
00593
00594 for(unsigned int i = 0; i < place_locations.size(); i++) {
00595
00596 if(execution_info[i].result_.result_code != 0) continue;
00597
00598 if(execution_info[i].descend_trajectory_.points.empty()) {
00599 ROS_WARN_STREAM("No result code and no points in approach trajectory");
00600 continue;
00601 }
00602
00603 std::map<std::string, double> pre_place_ik = grasp_joint_vals;
00604 for(unsigned int j = 0; j < joint_names.size(); j++) {
00605 pre_place_ik[joint_names[j]] = execution_info[i].descend_trajectory_.points[0].positions[j];
00606 }
00607 state->setKinematicState(pre_place_ik);
00608 if(cm->isKinematicStateInCollision(*state)) {
00609 ROS_DEBUG_STREAM("Final pre-place check failed");
00610 execution_info[i].result_.result_code = PlaceLocationResult::PREPLACE_OUT_OF_REACH;
00611 outcome_count[PlaceLocationResult::PREPLACE_OUT_OF_REACH]++;
00612 continue;
00613 }
00614
00615 }
00616
00617
00618 cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00619
00620 for(unsigned int i = 0; i < place_locations.size(); i++) {
00621
00622 if(execution_info[i].result_.result_code != 0) continue;
00623
00624 if(execution_info[i].retreat_trajectory_.points.empty()) {
00625 ROS_WARN_STREAM("No result code and no points in retreat trajectory");
00626 continue;
00627 }
00628 std::map<std::string, double> retreat_ik = post_grasp_joint_vals;
00629 for(unsigned int j = 0; j < joint_names.size(); j++) {
00630 retreat_ik[joint_names[j]] = execution_info[i].retreat_trajectory_.points.back().positions[j];
00631 }
00632 state->setKinematicState(retreat_ik);
00633 if(cm->isKinematicStateInCollision(*state)) {
00634 ROS_DEBUG_STREAM("Final lift check failed");
00635 execution_info[i].result_.result_code = PlaceLocationResult::RETREAT_OUT_OF_REACH;
00636 outcome_count[PlaceLocationResult::RETREAT_OUT_OF_REACH]++;
00637 continue;
00638 } else {
00639 ROS_DEBUG_STREAM("Everything successful");
00640 execution_info[i].result_.result_code = PlaceLocationResult::SUCCESS;
00641 outcome_count[PlaceLocationResult::SUCCESS]++;
00642 }
00643 }
00644 cm->setAlteredAllowedCollisionMatrix(original_acm);
00645
00646 ROS_INFO_STREAM("Took " << (ros::WallTime::now()-start).toSec());
00647
00648 for(std::map<unsigned int, unsigned int>::iterator it = outcome_count.begin();
00649 it != outcome_count.end();
00650 it++) {
00651 ROS_INFO_STREAM("Outcome " << it->first << " count " << it->second);
00652 }
00653 }
00654
00655
00656 }