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
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
00054
00055 }
00056
00057 PlaceSequenceValidator::~PlaceSequenceValidator() {
00058
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
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
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
00154 std::vector<tf::Transform> gripperTcpInWorldPoses(place_locations.size());
00155
00156
00157 for(unsigned int i = 0; i < place_locations.size(); i++)
00158 {
00159
00160 state->setKinematicState(post_grasp_joint_vals);
00161
00162
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;
00176 tf::poseMsgToTF(place_goal.grasp.grasp_pose, gripperInObjPose);
00177
00178
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
00194 execution_info[i].result_.result_code = 0;
00195 }
00196 }
00197
00198
00199 cm->revertCollisionSpacePaddingToDefault();
00200
00201
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
00225 }
00226 }
00227
00228
00229
00230
00231 cm->setAlteredAllowedCollisionMatrix(object_all_arm_disable_acm);
00232
00233
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
00247
00248
00249
00250
00251
00252
00253
00254
00255
00256
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
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
00286 cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00287
00288
00289 cm->applyLinkPaddingToCollisionSpace(linkPaddingForPlace(place_goal));
00290 }
00291
00292 state->setKinematicState(planning_scene_state_values_post_grasp);
00293
00294
00295 geometry_msgs::Pose place_geom_pose;
00296 tf::poseTFToMsg(gripperTcpInWorldPoses[i]*_TcpToWristTransform, place_geom_pose);
00297
00298 geometry_msgs::PoseStamped wristInArmBasePose;
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
00330 tf::Transform wristInArmBaseTransform;
00331 tf::poseMsgToTF(wristInArmBasePose.pose, wristInArmBaseTransform);
00332
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
00376 cm->revertCollisionSpacePaddingToDefault();
00377
00378
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
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
00443 cm->setAlteredAllowedCollisionMatrix(object_support_disable_acm);
00444
00445
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
00454 state->setKinematicState(planning_scene_state_values_post_grasp);
00455
00456
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
00485 tf::Transform wristInArmBaseTransform;
00486 tf::poseMsgToTF(wristInArmBasePose.pose, wristInArmBaseTransform);
00487
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
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
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 }