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 #include <object_manipulator/place_execution/descend_retreat_place.h>
00035
00036 #include "object_manipulator/tools/mechanism_interface.h"
00037 #include "object_manipulator/tools/exceptions.h"
00038 #include "object_manipulator/tools/hand_description.h"
00039 #include "object_manipulator/tools/vector_tools.h"
00040
00041 using arm_navigation_msgs::ArmNavigationErrorCodes;
00042 using object_manipulation_msgs::PlaceLocationResult;
00043
00044 namespace object_manipulator {
00045
00047 float StandardPlaceTester::EPS = 1.0e-6;
00048
00049
00050
00051 void PlaceTester::testPlaces(const object_manipulation_msgs::PlaceGoal &goal,
00052 const std::vector<geometry_msgs::PoseStamped> &place_locations,
00053 std::vector<PlaceExecutionInfo> &execution_info,
00054 bool return_on_first_hit)
00055 {
00056 execution_info.clear();
00057 for (size_t i=0; i<place_locations.size(); i++)
00058 {
00059 ROS_DEBUG_NAMED("manipulation","Place tester: testing place %zd out of batch of %zd", i, place_locations.size());
00060 if (feedback_function_) feedback_function_(i);
00061 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00062 PlaceExecutionInfo info;
00063 info.gripper_place_pose_ = place_locations[i];
00064 if (marker_publisher_)
00065 {
00066 info.marker_id_ = marker_publisher_->addGraspMarker(info.gripper_place_pose_);
00067 marker_publisher_->colorGraspMarker(info.marker_id_, 1.0, 0.0, 1.0);
00068 }
00069 testPlace(goal, place_locations[i], info);
00070 execution_info.push_back(info);
00071 if (info.result_.result_code == info.result_.SUCCESS && return_on_first_hit) return;
00072 }
00073 }
00074
00075 void PlacePerformer::performPlaces(const object_manipulation_msgs::PlaceGoal &goal,
00076 const std::vector<geometry_msgs::PoseStamped> &place_locations,
00077 std::vector<PlaceExecutionInfo> &execution_info)
00078 {
00079 for (size_t i=0; i<place_locations.size(); i++)
00080 {
00081 if (feedback_function_) feedback_function_(i);
00082 if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00083 if (i>= execution_info.size()) throw GraspException("Place Performer: not enough execution info provided");
00084 if (execution_info[i].result_.result_code != PlaceLocationResult::SUCCESS) continue;
00085 ROS_DEBUG_NAMED("manipulation","Place performer: trying place %zd out of batch of %zd", i, place_locations.size());
00086 performPlace(goal, place_locations[i], execution_info[i]);
00087 if (execution_info[i].result_.result_code == execution_info[i].result_.SUCCESS ||
00088 !execution_info[i].result_.continuation_possible) return;
00089 }
00090 }
00091
00092
00093
00094 void StandardPlaceTester::testPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00095 const geometry_msgs::PoseStamped &place_location,
00096 PlaceExecutionInfo &execution_info)
00097 {
00098
00099 arm_navigation_msgs::OrderedCollisionOperations ord;
00100 arm_navigation_msgs::CollisionOperation coll;
00101 coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00102 if (!place_goal.collision_object_name.empty() && !place_goal.collision_support_surface_name.empty())
00103 {
00104 coll.object1 = place_goal.collision_object_name;
00105 coll.object2 = place_goal.collision_support_surface_name;
00106 ord.collision_operations.push_back(coll);
00107 }
00108 if (place_goal.allow_gripper_support_collision)
00109 {
00110 coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00111 coll.object2 = place_goal.collision_support_surface_name;
00112 ord.collision_operations.push_back(coll);
00113 }
00114 ord.collision_operations = concat(ord.collision_operations,
00115 place_goal.additional_collision_operations.collision_operations);
00116
00117
00118
00119
00120 std::vector<arm_navigation_msgs::LinkPadding> link_padding =
00121 MechanismInterface::gripperPadding(place_goal.arm_name, 0.0);
00122
00123
00124 arm_navigation_msgs::LinkPadding padding;
00125 padding.link_name = handDescription().attachedName(place_goal.arm_name);
00126 padding.padding = place_goal.place_padding;
00127 link_padding.push_back(padding);
00128 link_padding = concat(link_padding, place_goal.additional_link_padding);
00129
00130 geometry_msgs::Vector3Stamped place_direction;
00131 place_direction.header.frame_id = place_goal.approach.direction.header.frame_id;
00132 place_direction.header.stamp = ros::Time::now();
00133 place_direction.vector = mechInterface().negate(place_goal.approach.direction.vector);
00134
00135
00136 std::vector<double> empty;
00137 float actual_distance;
00138 int error_code = mechInterface().getInterpolatedIK(place_goal.arm_name,
00139 execution_info.gripper_place_pose_, place_direction,
00140 place_goal.approach.desired_distance,
00141 empty,
00142 place_goal.grasp.grasp_posture,
00143 ord, link_padding,
00144 true, execution_info.descend_trajectory_, actual_distance);
00145 ROS_DEBUG_NAMED("manipulation"," Place trajectory: actual(%f), min(%f), desired (%f)",
00146 actual_distance, place_goal.approach.min_distance, place_goal.approach.desired_distance);
00147
00148 if (actual_distance < place_goal.approach.min_distance - EPS)
00149 {
00150 ROS_DEBUG_NAMED("manipulation","Place trajectory below min. threshold");
00151 if (execution_info.descend_trajectory_.points.empty())
00152 {
00153 ROS_DEBUG_NAMED("manipulation","Place trajectory empty; problem is with place location");
00154 if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00155 execution_info.result_ = Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00156 return;
00157 }
00158 else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00159 execution_info.result_ = Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00160 return;
00161 }
00162 else {
00163 execution_info.result_ = Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);
00164 return;
00165 }
00166 }
00167 if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00168 execution_info.result_ = Result(PlaceLocationResult::PREPLACE_IN_COLLISION, true);
00169 return;
00170 }
00171 else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00172 execution_info.result_ = Result(PlaceLocationResult::PREPLACE_OUT_OF_REACH, true);
00173 return;
00174 }
00175 else {
00176 execution_info.result_ = Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);
00177 return;
00178 }
00179 }
00180
00181
00182
00183 if ( !mechInterface().checkStateValidity(place_goal.arm_name,
00184 execution_info.descend_trajectory_.points.front().positions,
00185 place_goal.additional_collision_operations,
00186 place_goal.additional_link_padding) )
00187 {
00188 ROS_DEBUG_NAMED("manipulation","First pose in place trajectory is unfeasible with default padding");
00189 execution_info.result_ = Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);
00190 return;
00191 }
00192
00193
00194 ord.collision_operations.clear();
00195 coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00196
00197 if (!place_goal.collision_object_name.empty())
00198 {
00199 coll.object1 = place_goal.collision_object_name;
00200 coll.object2 = coll.COLLISION_SET_ALL;
00201 ord.collision_operations.push_back(coll);
00202 }
00203 if (place_goal.allow_gripper_support_collision)
00204 {
00205 coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00206 coll.object2 = place_goal.collision_support_surface_name;
00207 ord.collision_operations.push_back(coll);
00208 }
00209 ord.collision_operations = concat(ord.collision_operations,
00210 place_goal.additional_collision_operations.collision_operations);
00211
00212 geometry_msgs::Vector3Stamped retreat_direction;
00213 retreat_direction.header.stamp = ros::Time::now();
00214 retreat_direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00215 retreat_direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00216
00217
00218 std::vector<double> place_joint_angles = execution_info.descend_trajectory_.points.back().positions;
00219 mechInterface().getInterpolatedIK(place_goal.arm_name,
00220 execution_info.gripper_place_pose_, retreat_direction,
00221 place_goal.desired_retreat_distance,
00222 place_joint_angles,
00223 place_goal.grasp.pre_grasp_posture,
00224 ord, link_padding,
00225 false, execution_info.retreat_trajectory_, actual_distance);
00226 ROS_DEBUG_NAMED("manipulation","Retreat trajectory: actual (%f), min (%f) and desired (%f)", actual_distance,
00227 place_goal.min_retreat_distance, place_goal.desired_retreat_distance);
00228
00229 if (actual_distance < place_goal.min_retreat_distance - EPS)
00230 {
00231 ROS_DEBUG_NAMED("manipulation","Retreat trajectory below min. threshold");
00232 if (execution_info.retreat_trajectory_.points.empty())
00233 {
00234 ROS_DEBUG_NAMED("manipulation","Retreat trajectory empty; problem is with place location");
00235 if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00236 execution_info.result_ = Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00237 return;
00238 }
00239 else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00240 execution_info.result_ = Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00241 return;
00242 }
00243 else {
00244 execution_info.result_ = Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);
00245 return;
00246 }
00247 }
00248 if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00249 execution_info.result_ = Result(PlaceLocationResult::RETREAT_IN_COLLISION, true);
00250 return;
00251 }
00252 else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00253 execution_info.result_ = Result(PlaceLocationResult::RETREAT_OUT_OF_REACH, true);
00254 return;
00255 }
00256 else {
00257 execution_info.result_ = Result(PlaceLocationResult::RETREAT_UNFEASIBLE, true);
00258 return;
00259 }
00260 }
00261 execution_info.result_ = Result(PlaceLocationResult::SUCCESS, true);
00262 }
00263
00264
00265
00280 PlaceLocationResult StandardPlacePerformer::retreat(const object_manipulation_msgs::PlaceGoal &place_goal)
00281 {
00282 arm_navigation_msgs::OrderedCollisionOperations ord;
00283 arm_navigation_msgs::CollisionOperation coll;
00284 coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00285 coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00286
00287 if (!place_goal.collision_object_name.empty())
00288 {
00289 coll.object2 = place_goal.collision_object_name;
00290 ord.collision_operations.push_back(coll);
00291 }
00292
00293 if (!place_goal.collision_support_surface_name.empty())
00294 {
00295 coll.object2 = place_goal.collision_support_surface_name;
00296 ord.collision_operations.push_back(coll);
00297 }
00298 ord.collision_operations = concat(ord.collision_operations,
00299 place_goal.additional_collision_operations.collision_operations);
00300
00301
00302 std::vector<arm_navigation_msgs::LinkPadding> link_padding
00303 = concat(MechanismInterface::gripperPadding(place_goal.arm_name, 0.0),
00304 place_goal.additional_link_padding);
00305
00306 geometry_msgs::Vector3Stamped direction;
00307 direction.header.stamp = ros::Time::now();
00308 direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00309 direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00310
00311 float actual_distance;
00312 mechInterface().translateGripper(place_goal.arm_name,
00313 direction,
00314 ord, link_padding,
00315 place_goal.desired_retreat_distance, 0, actual_distance);
00316 if (actual_distance < place_goal.min_retreat_distance)
00317 {
00318 ROS_DEBUG_NAMED("manipulation","Object place: retreat incomplete (%f executed and %f desired)",
00319 actual_distance, place_goal.min_retreat_distance);
00320 return Result(PlaceLocationResult::RETREAT_FAILED, false);
00321 }
00322
00323 return Result(PlaceLocationResult::SUCCESS, true);
00324 }
00325
00329 PlaceLocationResult
00330 StandardPlacePerformer::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00331 const PlaceExecutionInfo &execution_info)
00332 {
00333 mechInterface().attemptTrajectory(place_goal.arm_name, execution_info.descend_trajectory_, true);
00334 return Result(PlaceLocationResult::SUCCESS, true);
00335 }
00336
00337
00338 void StandardPlacePerformer::performPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00339 const geometry_msgs::PoseStamped &place_location,
00340 PlaceExecutionInfo &execution_info)
00341 {
00342
00343 bool use_constraints = true;
00344
00345 if(!constraintsUnderstandable(place_goal.path_constraints))
00346 {
00347 ROS_WARN("Constraints passed to place executor are of types not yet handled. Ignoring them.");
00348 use_constraints = false;
00349 }
00350 if (place_goal.path_constraints.orientation_constraints.empty())
00351 {
00352 use_constraints = false;
00353 }
00354 if(use_constraints)
00355 {
00356
00357 geometry_msgs::PoseStamped place_pose;
00358 place_pose.header.frame_id = place_location.header.frame_id;
00359 place_pose.header.stamp = ros::Time(0.0);
00360
00361 if(!mechInterface().getFK(place_goal.arm_name, execution_info.descend_trajectory_.points.front().positions,
00362 place_pose))
00363 {
00364 ROS_ERROR("Could not re-compute pre-place pose based on trajectory");
00365 throw MechanismException("Could not re-compute pre-place pose based on trajectory");
00366 }
00367 ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place with constraints");
00368 if (!mechInterface().moveArmConstrained(place_goal.arm_name, place_pose,
00369 place_goal.additional_collision_operations,
00370 place_goal.additional_link_padding,
00371 place_goal.path_constraints,
00372 execution_info.descend_trajectory_.points.front().positions[2],
00373 false))
00374 {
00375
00376
00377 ROS_WARN("Object place: move_arm to pre-place with constraints failed. Trying again without constraints.");
00378 use_constraints = false;
00379 }
00380 }
00381
00382 if(!use_constraints)
00383 {
00384 ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place without constraints");
00385 if ( !mechInterface().attemptMoveArmToGoal(place_goal.arm_name,
00386 execution_info.descend_trajectory_.points.front().positions,
00387 place_goal.additional_collision_operations,
00388 place_goal.additional_link_padding) )
00389 {
00390 ROS_DEBUG_NAMED("manipulation","Object place: move_arm (without constraints) to pre-place reports failure");
00391 execution_info.result_ = Result(PlaceLocationResult::MOVE_ARM_FAILED, true);
00392 return;
00393 }
00394 }
00395 ROS_DEBUG_NAMED("manipulation"," Arm moved to pre-place");
00396
00397 execution_info.result_ = placeApproach(place_goal, execution_info);
00398 if ( execution_info.result_.result_code != PlaceLocationResult::SUCCESS)
00399 {
00400 ROS_DEBUG_NAMED("manipulation"," Pre-place to place approach failed");
00401 execution_info.result_ = Result(PlaceLocationResult::PLACE_FAILED, false);
00402 return;
00403 }
00404 ROS_DEBUG_NAMED("manipulation"," Place trajectory done");
00405
00406 mechInterface().detachAndAddBackObjectsAttachedToGripper(place_goal.arm_name, place_goal.collision_object_name);
00407 ROS_DEBUG_NAMED("manipulation"," Object detached");
00408
00409 mechInterface().handPostureGraspAction(place_goal.arm_name, place_goal.grasp,
00410 object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, -1);
00411 ROS_DEBUG_NAMED("manipulation"," Object released");
00412
00413 execution_info.result_ = retreat(place_goal);
00414 if (execution_info.result_.result_code != PlaceLocationResult::SUCCESS)
00415 {
00416 execution_info.result_ = Result(PlaceLocationResult::RETREAT_FAILED, false);
00417 return;
00418 }
00419 execution_info.result_ = Result(PlaceLocationResult::SUCCESS, true);
00420 }
00421
00425 bool StandardPlacePerformer::constraintsUnderstandable(const arm_navigation_msgs::Constraints &constraints)
00426 {
00427 if(constraints.position_constraints.empty() && constraints.orientation_constraints.empty() &&
00428 constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00429 return true;
00430 if(constraints.orientation_constraints.size() == 1 && constraints.position_constraints.empty()
00431 && constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00432 return true;
00433 return false;
00434 }
00435
00436 PlaceLocationResult
00437 ReactivePlacePerformer::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00438 const PlaceExecutionInfo &execution_info)
00439 {
00440
00441 object_manipulation_msgs::ReactivePlaceGoal reactive_place_goal;
00442 reactive_place_goal.arm_name = place_goal.arm_name;
00443 reactive_place_goal.collision_object_name = place_goal.collision_object_name;
00444 reactive_place_goal.collision_support_surface_name = place_goal.collision_support_surface_name;
00445 reactive_place_goal.trajectory = execution_info.descend_trajectory_;
00446 reactive_place_goal.final_place_pose = execution_info.gripper_place_pose_;
00447
00448
00449 ros::Duration timeout = ros::Duration(60.0);
00450 ROS_DEBUG_NAMED("manipulation"," Calling the reactive place action");
00451 mechInterface().reactive_place_action_client_.client(place_goal.arm_name).sendGoal(reactive_place_goal);
00452 if ( !mechInterface().reactive_place_action_client_.client(place_goal.arm_name).waitForResult(timeout) )
00453 {
00454 ROS_ERROR(" Reactive place timed out");
00455 return Result(PlaceLocationResult::PLACE_FAILED, false);
00456 }
00457 object_manipulation_msgs::ReactivePlaceResult reactive_place_result =
00458 *mechInterface().reactive_place_action_client_.client(place_goal.arm_name).getResult();
00459 if (reactive_place_result.manipulation_result.value != reactive_place_result.manipulation_result.SUCCESS)
00460 {
00461 ROS_ERROR(" Reactive place failed with error code %d", reactive_place_result.manipulation_result.value);
00462 return Result(PlaceLocationResult::PLACE_FAILED, false);
00463 }
00464 ROS_DEBUG_NAMED("manipulation"," Reactive place action succeeded");
00465 return Result(PlaceLocationResult::SUCCESS, true);
00466 }
00467
00468 }