object_manipulator: object_manipulator: approach_lift

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 #include "object_manipulator/grasp_execution/approach_lift_grasp.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 object_manipulation_msgs::GraspResult;
00042 using arm_navigation_msgs::ArmNavigationErrorCodes;
00043 
00044 namespace object_manipulator {
00045 
00047 float GraspTesterWithApproach::EPS = 1.0e-6;
00048 
00049 // ---------------------------- Definitions ---------------------------------
00050 
00051 void GraspTester::testGrasps(const object_manipulation_msgs::PickupGoal &goal,
00052                              const std::vector<object_manipulation_msgs::Grasp> &grasps,
00053                              std::vector<GraspExecutionInfo> &execution_info,
00054                              bool return_on_first_hit)
00055 {
00056   execution_info.clear();
00057   for (size_t i=0; i<grasps.size(); i++)
00058   {
00059     GraspExecutionInfo info;
00060     ROS_DEBUG_NAMED("manipulation","Grasp tester: testing grasp %zd out of batch of %zd", i, grasps.size());
00061     if (feedback_function_) feedback_function_(i);
00062     if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();   
00063     if (marker_publisher_)
00064     {
00065       geometry_msgs::PoseStamped marker_pose;
00066       marker_pose.pose = grasps[i].grasp_pose;
00067       marker_pose.header.frame_id = goal.target.reference_frame_id;
00068       marker_pose.header.stamp = ros::Time::now();
00069       info.marker_id_ = marker_publisher_->addGraspMarker(marker_pose);
00070     }  
00071     testGrasp(goal, grasps[i], info);
00072     execution_info.push_back(info);
00073     if (info.result_.result_code == info.result_.SUCCESS && return_on_first_hit) return;
00074   }
00075 }
00076 
00077 void GraspPerformer::performGrasps(const object_manipulation_msgs::PickupGoal &goal,
00078                                    const std::vector<object_manipulation_msgs::Grasp> &grasps,
00079                                    std::vector<GraspExecutionInfo> &execution_info)
00080 {
00081   for (size_t i=0; i<grasps.size(); i++)
00082   {
00083     if (feedback_function_) feedback_function_(i);
00084     if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00085     if (i>= execution_info.size()) throw GraspException("Grasp Performer: not enough execution info provided");
00086     if (execution_info[i].result_.result_code != GraspResult::SUCCESS) continue;
00087     ROS_DEBUG_NAMED("manipulation","Grasp performer: trying grasp %zd out of batch of %zd", i, grasps.size());
00088     performGrasp(goal, grasps[i], execution_info[i]);
00089     if (execution_info[i].result_.result_code == execution_info[i].result_.SUCCESS || 
00090         !execution_info[i].result_.continuation_possible) return;
00091   }
00092 }
00093 
00094 // ------------------------------ Grasp Testers ----------------------------------
00095 
00099 arm_navigation_msgs::OrderedCollisionOperations 
00100 GraspTesterWithApproach::collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00101                                                     const object_manipulation_msgs::Grasp &grasp)
00102 {
00103   arm_navigation_msgs::OrderedCollisionOperations ord;
00104   arm_navigation_msgs::CollisionOperation coll;
00105   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00106   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00107   if (!pickup_goal.collision_object_name.empty())
00108   {
00109     coll.object2 = pickup_goal.collision_object_name;
00110     ord.collision_operations.push_back(coll);
00111   }
00112   if (pickup_goal.allow_gripper_support_collision)
00113   {
00114     coll.object2 = pickup_goal.collision_support_surface_name;
00115     ord.collision_operations.push_back(coll);
00116   }
00117 
00118   for (unsigned int i = 0; i < grasp.moved_obstacles.size(); i++)
00119   {
00120     ROS_DEBUG_NAMED("manipulation","  Disabling all collisions for lift against moved obstacle %s",
00121                      grasp.moved_obstacles[i].collision_name.c_str());
00122     coll.object1 = grasp.moved_obstacles[i].collision_name;
00123     //This disables all collisions with the object. Ignore possible collisions of the robot with the object 
00124     //during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable 
00125     //collisions just with the forearm, which should work almost always but still theoretically is not the right 
00126     //thing to do.
00127     coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00128     ord.collision_operations.push_back(coll);
00129   }
00130 
00131   ord.collision_operations = concat(ord.collision_operations, 
00132                                     pickup_goal.additional_collision_operations.collision_operations);  
00133   return ord;
00134 }
00135 
00137 std::vector<arm_navigation_msgs::LinkPadding> 
00138 GraspTesterWithApproach::linkPaddingForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00139                                             const object_manipulation_msgs::Grasp&)
00140 {
00141   return concat(MechanismInterface::gripperPadding(pickup_goal.arm_name, 0.0), 
00142                 pickup_goal.additional_link_padding);
00143 }
00144 
00145 GraspResult 
00146 GraspTesterWithApproach::getInterpolatedIKForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00147                                                   const object_manipulation_msgs::Grasp &grasp,
00148                                                   const std::vector<double> &grasp_joint_angles,
00149                                                   GraspExecutionInfo &execution_info)
00150 {
00151   geometry_msgs::PoseStamped grasp_pose;
00152   grasp_pose.pose = grasp.grasp_pose;
00153   grasp_pose.header.frame_id = pickup_goal.target.reference_frame_id;
00154   grasp_pose.header.stamp = ros::Time(0);
00155 
00156   float actual_lift_distance;
00157   int error_code = 
00158     mechInterface().getInterpolatedIK(pickup_goal.arm_name, 
00159                                       grasp_pose, 
00160                                       pickup_goal.lift.direction, pickup_goal.lift.desired_distance,
00161                                       grasp_joint_angles, 
00162                                       grasp.grasp_posture, 
00163                                       collisionOperationsForLift(pickup_goal, grasp), 
00164                                       linkPaddingForLift(pickup_goal, grasp),
00165                                       false, execution_info.lift_trajectory_, actual_lift_distance);
00166   ROS_DEBUG_NAMED("manipulation","  Lift distance: actual %f, min %f and desired %f", 
00167                   actual_lift_distance, pickup_goal.lift.min_distance, pickup_goal.lift.desired_distance);
00168 
00169   if (actual_lift_distance < pickup_goal.lift.min_distance - EPS)
00170   {
00171     ROS_DEBUG_NAMED("manipulation","  Lift trajectory  below min. threshold");
00172     if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 0.0, 1.0);
00173     if (execution_info.lift_trajectory_.points.empty())
00174     {
00175       ROS_DEBUG_NAMED("manipulation","  Lift trajectory empty; problem is with grasp location");
00176       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00177         return Result(GraspResult::GRASP_IN_COLLISION, true);
00178       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00179         return Result(GraspResult::GRASP_OUT_OF_REACH, true);
00180       else return Result(GraspResult::GRASP_UNFEASIBLE, true);
00181     }
00182     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00183       return Result(GraspResult::LIFT_IN_COLLISION, true);
00184     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00185       return Result(GraspResult::LIFT_OUT_OF_REACH, true);
00186     else return Result(GraspResult::LIFT_UNFEASIBLE, true);
00187   }
00188  
00189   return Result(GraspResult::SUCCESS, true);
00190 }
00191 
00195 arm_navigation_msgs::OrderedCollisionOperations 
00196 GraspTesterWithApproach::collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00197                                                      const object_manipulation_msgs::Grasp &grasp)
00198 {
00199   arm_navigation_msgs::OrderedCollisionOperations ord;
00200   arm_navigation_msgs::CollisionOperation coll;
00201   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00202   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00203   if (!pickup_goal.collision_object_name.empty())
00204   {
00205     coll.object2 = pickup_goal.collision_object_name;
00206     ord.collision_operations.push_back(coll);
00207   }
00208   if (pickup_goal.allow_gripper_support_collision)
00209   {
00210     coll.object2 = pickup_goal.collision_support_surface_name;
00211     ord.collision_operations.push_back(coll);
00212   }
00213 
00214   coll.object1 = pickup_goal.collision_object_name;
00215   //This disables all collisions with the object. Ignore possible collisions of the robot with the object 
00216   //during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable 
00217   //collisions just with the forearm, which should work almost always but still theoretially is not the right 
00218   //thing to do.
00219   coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00220   ord.collision_operations.push_back(coll);
00221 
00222   for (unsigned int i = 0; i < grasp.moved_obstacles.size(); i++)
00223   {
00224     ROS_DEBUG_NAMED("manipulation","  Disabling all collisions for grasp against moved obstacle %s",
00225                      grasp.moved_obstacles[i].collision_name.c_str());
00226     coll.object1 = grasp.moved_obstacles[i].collision_name;
00227     //This disables all collisions with the object. Ignore possible collisions of the robot with the object 
00228     //during the push-grasp. We need all the capture region to be able to do this safely. Or we can disable 
00229     //collisions just with the forearm, which should work almost always but still theoretially is not the right 
00230     //thing to do.
00231     coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00232     ord.collision_operations.push_back(coll);
00233   }
00234 
00235   ord.collision_operations = concat(ord.collision_operations, 
00236                                     pickup_goal.additional_collision_operations.collision_operations);
00237   return ord;
00238 }
00239 
00241 std::vector<arm_navigation_msgs::LinkPadding> 
00242 GraspTesterWithApproach::linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00243                                              const object_manipulation_msgs::Grasp&)
00244 {
00245   return concat(MechanismInterface::gripperPadding(pickup_goal.arm_name, 0.0), 
00246                 pickup_goal.additional_link_padding);
00247 }
00248 
00249 GraspResult 
00250 GraspTesterWithApproach::getInterpolatedIKForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00251                                                    const object_manipulation_msgs::Grasp &grasp,
00252                                                    GraspExecutionInfo &execution_info)
00253 {
00254   //get the grasp pose in the right frame
00255   geometry_msgs::PoseStamped grasp_pose_stamped;
00256   grasp_pose_stamped.pose = grasp.grasp_pose;
00257   grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
00258   grasp_pose_stamped.header.stamp = ros::Time(0);
00259 
00260   //use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp
00261   geometry_msgs::Vector3Stamped direction;
00262   direction.header.stamp = ros::Time::now();
00263   direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
00264   direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
00265 
00266   std::vector<double> empty;
00267   //remember to pass that we want to flip the trajectory
00268   float actual_approach_distance;
00269   int error_code = mechInterface().getInterpolatedIK(pickup_goal.arm_name, 
00270                                                      grasp_pose_stamped, 
00271                                                      direction, grasp.desired_approach_distance,
00272                                                      empty, 
00273                                                      grasp.pre_grasp_posture,
00274                                                      collisionOperationsForGrasp(pickup_goal, grasp), 
00275                                                      linkPaddingForGrasp(pickup_goal, grasp),
00276                                                      true, execution_info.approach_trajectory_, 
00277                                                      actual_approach_distance);
00278   ROS_DEBUG_NAMED("manipulation","  Grasp executor approach distance: actual (%f), min(%f) and desired (%f)", 
00279             actual_approach_distance, grasp.min_approach_distance, grasp.desired_approach_distance);
00280 
00281   if ( actual_approach_distance < grasp.min_approach_distance - EPS)
00282   {
00283     ROS_DEBUG_NAMED("manipulation","  Grasp executor: interpolated IK for grasp below min threshold");
00284     if (execution_info.approach_trajectory_.points.empty())
00285     {
00286       ROS_DEBUG_NAMED("manipulation","  Grasp executor: interpolated IK empty, problem is with grasp location");
00287       if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 1.0, 1.0, 0.0); //yellow
00288       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00289         return Result(GraspResult::GRASP_IN_COLLISION, true);
00290       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00291         return Result(GraspResult::GRASP_OUT_OF_REACH, true);
00292       else return Result(GraspResult::GRASP_UNFEASIBLE, true);      
00293     }
00294     if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 1.0); //cyan
00295     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00296       return Result(GraspResult::PREGRASP_IN_COLLISION, true);
00297     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00298       return Result(GraspResult::PREGRASP_OUT_OF_REACH, true);
00299     else return Result(GraspResult::PREGRASP_UNFEASIBLE, true);      
00300   }
00301 
00302   return Result(GraspResult::SUCCESS, true);
00303 }
00304 
00305 void GraspTesterWithApproach::testGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00306                                         const object_manipulation_msgs::Grasp &grasp,
00307                                         GraspExecutionInfo &execution_info)
00308 {
00309   if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 1.0, 0.0, 0.0); //red
00310 
00311   execution_info.result_ = getInterpolatedIKForGrasp(pickup_goal, grasp, execution_info);
00312   if ( execution_info.result_.result_code != GraspResult::SUCCESS )
00313   {
00314     ROS_DEBUG_NAMED("manipulation","  Grasp executor: failed to generate approach trajectory");
00315     return;
00316   }
00317 
00318   //check for lift trajectory starting from grasp solution
00319   std::vector<double> grasp_joint_angles = execution_info.approach_trajectory_.points.back().positions;
00320   execution_info.result_ = getInterpolatedIKForLift(pickup_goal, grasp, grasp_joint_angles, execution_info);
00321   if (execution_info.result_.result_code != GraspResult::SUCCESS)
00322   {
00323     ROS_DEBUG_NAMED("manipulation","  Grasp executor: failed to generate lift trajectory");
00324     return;
00325   }
00326 
00327   //check if the first pose in grasp trajectory is valid
00328   //when we check from pre-grasp to grasp we use custom link padding, so we need to check here
00329   //if the initial pose is feasible with default padding; otherwise, move_arm might refuse to 
00330   //take us there
00331   if ( !mechInterface().checkStateValidity(pickup_goal.arm_name, 
00332                                            execution_info.approach_trajectory_.points.front().positions,
00333                                            pickup_goal.additional_collision_operations,
00334                                            pickup_goal.additional_link_padding) )
00335   {
00336     ROS_DEBUG_NAMED("manipulation","  Grasp executor: initial pose in grasp trajectory is unfeasible "
00337                     "with default padding");
00338     execution_info.result_ = Result(GraspResult::PREGRASP_UNFEASIBLE, true);
00339     return;
00340   }
00341   ROS_DEBUG_NAMED("manipulation","  Grasp executor: goal pose for move arm passed validity test");
00342 
00343   //check if the last pose in lift trajectory is valid
00344   //when we check for lift we use custom link padding, so we need to check here if the last pose 
00345   //is feasible with default padding; otherwise, move_arm might refuse to take us out of there
00346   if ( pickup_goal.lift.min_distance != 0 && 
00347        !mechInterface().checkStateValidity(pickup_goal.arm_name, 
00348                                            execution_info.lift_trajectory_.points.back().positions,
00349                                            collisionOperationsForLift(pickup_goal, grasp),
00350                                            pickup_goal.additional_link_padding) )
00351   {
00352     ROS_DEBUG_NAMED("manipulation","  Grasp executor: last pose in lift trajectory is unfeasible with default padding");
00353     execution_info.result_ = Result(GraspResult::LIFT_UNFEASIBLE, true);
00354     return;
00355   }  
00356   ROS_DEBUG_NAMED("manipulation","  Grasp executor: final lift pose passed validity test");
00357 }
00358 
00360 arm_navigation_msgs::OrderedCollisionOperations 
00361 UnsafeGraspTester::collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00362                                                const object_manipulation_msgs::Grasp&)
00363 {
00364   arm_navigation_msgs::OrderedCollisionOperations ord;
00365   arm_navigation_msgs::CollisionOperation coll;
00366   coll.object1 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00367   coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00368   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00369   ord.collision_operations.push_back(coll);
00370   ord.collision_operations = concat(ord.collision_operations, 
00371                                     pickup_goal.additional_collision_operations.collision_operations);
00372   return ord;
00373 }
00374 
00376 arm_navigation_msgs::OrderedCollisionOperations 
00377 UnsafeGraspTester::collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00378                                               const object_manipulation_msgs::Grasp&)
00379 {
00380   arm_navigation_msgs::OrderedCollisionOperations ord;
00381   arm_navigation_msgs::CollisionOperation coll;
00382   coll.object1 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00383   coll.object2 = arm_navigation_msgs::CollisionOperation::COLLISION_SET_ALL;
00384   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00385   ord.collision_operations.push_back(coll);
00386   ord.collision_operations = concat(ord.collision_operations, 
00387                                     pickup_goal.additional_collision_operations.collision_operations);
00388   return ord;
00389 }
00390 
00391 
00392 // ---------------------------- Grasp Performers ---------------------------------
00393 
00394 void StandardGraspPerformer::performGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00395                                           const object_manipulation_msgs::Grasp &grasp,
00396                                           GraspExecutionInfo &execution_info)
00397 {
00398   execution_info.result_ = approachAndGrasp(pickup_goal, grasp, execution_info);
00399   if (execution_info.result_.result_code != GraspResult::SUCCESS) return;
00400 
00401   //check if there is anything in gripper; if not, open gripper and retreat
00402   if (!mechInterface().graspPostureQuery(pickup_goal.arm_name, grasp))
00403   {
00404     ROS_DEBUG_NAMED("manipulation","Hand reports that grasp was not successfully executed; "
00405                     "releasing object and retreating");
00406     mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
00407                                 object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, -1);    
00408     retreat(pickup_goal, grasp, execution_info);
00409     execution_info.result_ = Result(GraspResult::GRASP_FAILED, false);
00410     return;
00411   }
00412   
00413   //attach object to gripper
00414   if (!pickup_goal.collision_object_name.empty())
00415   {
00416     mechInterface().attachObjectToGripper(pickup_goal.arm_name, pickup_goal.collision_object_name);
00417   }
00418 
00419   execution_info.result_ = lift(pickup_goal, grasp, execution_info);
00420 }
00421 
00422 
00423 object_manipulation_msgs::GraspResult 
00424 StandardGraspPerformer::approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00425                                          const object_manipulation_msgs::Grasp &grasp,
00426                                          GraspExecutionInfo &execution_info)
00427 {
00428   ROS_INFO("Additional collision operations:");
00429   for (unsigned int i = 0; i < pickup_goal.additional_collision_operations.collision_operations.size(); i++)
00430   {
00431     ROS_INFO_STREAM("\t for objects " << pickup_goal.additional_collision_operations.collision_operations[i].object1 << " and " << pickup_goal.additional_collision_operations.collision_operations[i].object2 );
00432   }
00433   if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name, 
00434                                              execution_info.approach_trajectory_.points.front().positions,
00435                                              pickup_goal.additional_collision_operations,
00436                                              pickup_goal.additional_link_padding) ) 
00437   {
00438     ROS_DEBUG_NAMED("manipulation","  Grasp executor: move_arm to pre-grasp reports failure");
00439     if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 1.0, 0.5, 0.0); //orange-ish
00440     return Result(GraspResult::MOVE_ARM_FAILED, true);
00441   }
00442 
00443   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00444                      object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP, -1);
00445 
00446   mechInterface().attemptTrajectory(pickup_goal.arm_name, execution_info.approach_trajectory_, true);
00447 
00448   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00449                      object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP, pickup_goal.max_contact_force);    
00450 
00451   if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 0.0); //green
00452   return Result(GraspResult::SUCCESS, true);
00453 }
00454 
00455 object_manipulation_msgs::GraspResult 
00456 StandardGraspPerformer::lift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00457                              const object_manipulation_msgs::Grasp &grasp,
00458                              GraspExecutionInfo &execution_info)
00459 {
00460   if (execution_info.lift_trajectory_.points.empty())
00461   {
00462     ROS_ERROR("  Grasp executor: lift trajectory not set");
00463     return Result(GraspResult::LIFT_FAILED, false);
00464   }
00465   mechInterface().attemptTrajectory(pickup_goal.arm_name, execution_info.lift_trajectory_, true);
00466   return Result(GraspResult::SUCCESS, true);
00467 
00468 }
00469 
00470 object_manipulation_msgs::GraspResult 
00471 StandardGraspPerformer::retreat(const object_manipulation_msgs::PickupGoal &pickup_goal,
00472                                 const object_manipulation_msgs::Grasp &grasp,
00473                                 GraspExecutionInfo &execution_info)
00474 {
00475   arm_navigation_msgs::OrderedCollisionOperations ord;
00476   arm_navigation_msgs::CollisionOperation coll;
00477   //disable collision between gripper and object
00478   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00479   coll.object2 = pickup_goal.collision_object_name;
00480   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00481   ord.collision_operations.push_back(coll);
00482   //disable collision between gripper and table
00483   coll.object2 = pickup_goal.collision_support_surface_name;
00484   ord.collision_operations.push_back(coll);
00485   ord.collision_operations = concat(ord.collision_operations, 
00486                                     pickup_goal.additional_collision_operations.collision_operations);
00487 
00488   geometry_msgs::Vector3Stamped direction;
00489   direction.header.stamp = ros::Time::now();
00490   direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
00491   direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
00492 
00493   //even if the complete retreat trajectory is not possible, execute as many
00494   //steps as we can (pass min_distance = 0)
00495   float retreat_distance = grasp.min_approach_distance;
00496   float actual_distance;
00497   if (!mechInterface().translateGripper(pickup_goal.arm_name, direction,
00498                                         ord, pickup_goal.additional_link_padding, 
00499                                         retreat_distance, 0, actual_distance))
00500   {
00501     ROS_ERROR(" Grasp executor: failed to retreat gripper at all");
00502     return Result(GraspResult::RETREAT_FAILED, false);
00503   }
00504   if (actual_distance < retreat_distance)
00505   {
00506     ROS_WARN(" Grasp executor: only partial retreat (%f) succeeeded", actual_distance);
00507     return Result(GraspResult::RETREAT_FAILED, false);
00508   }
00509   return Result(GraspResult::SUCCESS, true);
00510 }
00511 
00512 object_manipulation_msgs::GraspResult 
00513 ReactiveGraspPerformer::nonReactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00514                                         const object_manipulation_msgs::Grasp &grasp,
00515                                         GraspExecutionInfo &execution_info)
00516 {
00517   arm_navigation_msgs::OrderedCollisionOperations ord;
00518   arm_navigation_msgs::CollisionOperation coll;
00519   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00520   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00521   //disable collisions between end-effector and table
00522   if (!pickup_goal.collision_support_surface_name.empty())
00523   {
00524     coll.object2 = pickup_goal.collision_support_surface_name;
00525     ord.collision_operations.push_back(coll);
00526   }
00527   //disable collisions between attached object and table
00528   if (!pickup_goal.collision_support_surface_name.empty() && !pickup_goal.collision_object_name.empty())
00529   {
00530     coll.object1 = pickup_goal.collision_object_name;
00531     coll.object2 = pickup_goal.collision_support_surface_name;
00532     ord.collision_operations.push_back(coll);
00533   }
00534   ord.collision_operations = concat(ord.collision_operations, 
00535                                     pickup_goal.additional_collision_operations.collision_operations);
00536 
00537   float actual_distance;
00538   if (!mechInterface().translateGripper(pickup_goal.arm_name, 
00539                                         pickup_goal.lift.direction, 
00540                                         ord, pickup_goal.additional_link_padding, 
00541                                         pickup_goal.lift.desired_distance, 0.0, actual_distance))
00542   {
00543     ROS_DEBUG_NAMED("manipulation","  Reactive grasp executor: lift performed no steps");
00544     return Result(GraspResult::LIFT_FAILED, false);
00545   }
00546   if (actual_distance < pickup_goal.lift.min_distance)
00547   {
00548     ROS_DEBUG_NAMED("manipulation","  Reactive grasp executor: lift distance below min threshold ");
00549     return Result(GraspResult::LIFT_FAILED, false);
00550   }
00551   if (actual_distance < pickup_goal.lift.desired_distance)
00552   {
00553     ROS_DEBUG_NAMED("manipulation","  Reactive grasp executor: lift distance below desired, but above min threshold");
00554   }
00555   else 
00556   {
00557     ROS_DEBUG_NAMED("manipulation","  Reactive grasp executor: desired lift distance executed");
00558   }
00559   return Result(GraspResult::SUCCESS, true);
00560 }
00561 
00562 object_manipulation_msgs::GraspResult 
00563 ReactiveGraspPerformer::reactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00564                                      const object_manipulation_msgs::Grasp &grasp,
00565                                      GraspExecutionInfo &execution_info)
00566 {
00567   object_manipulation_msgs::ReactiveLiftGoal reactive_lift_goal;
00568   reactive_lift_goal.lift = pickup_goal.lift;
00569   reactive_lift_goal.arm_name = pickup_goal.arm_name;
00570   reactive_lift_goal.target = pickup_goal.target;
00571   reactive_lift_goal.trajectory = execution_info.lift_trajectory_;
00572   reactive_lift_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
00573 
00574   //give the reactive lift 1 minute to do its thing
00575   ros::Duration timeout = ros::Duration(60.0);
00576   mechInterface().reactive_lift_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_lift_goal);
00577   if ( !mechInterface().reactive_lift_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
00578   {
00579     ROS_ERROR("  Reactive lift timed out");
00580     return Result(GraspResult::LIFT_FAILED, false);
00581   }
00582   object_manipulation_msgs::ReactiveLiftResult reactive_lift_result = 
00583     *mechInterface().reactive_lift_action_client_.client(pickup_goal.arm_name).getResult();
00584   if (reactive_lift_result.manipulation_result.value != reactive_lift_result.manipulation_result.SUCCESS)
00585   {
00586     ROS_ERROR("Reactive lift failed with error code %d", reactive_lift_result.manipulation_result.value);
00587     return Result(GraspResult::LIFT_FAILED, false);
00588   }
00589   return Result(GraspResult::SUCCESS, true);
00590 }
00591 
00592 object_manipulation_msgs::GraspResult 
00593 ReactiveGraspPerformer::lift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00594                              const object_manipulation_msgs::Grasp &grasp,
00595                              GraspExecutionInfo &execution_info)
00596 {
00597   if (pickup_goal.use_reactive_lift)
00598   {
00599     return reactiveLift(pickup_goal, grasp, execution_info);
00600   }
00601   else
00602   {
00603     return nonReactiveLift(pickup_goal, grasp, execution_info);
00604   }
00605 }
00606 
00607 object_manipulation_msgs::GraspResult 
00608 ReactiveGraspPerformer::approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00609                                          const object_manipulation_msgs::Grasp &grasp,
00610                                          GraspExecutionInfo &execution_info)
00611 {
00612   if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name, 
00613                                              execution_info.approach_trajectory_.points.front().positions,
00614                                              pickup_goal.additional_collision_operations,
00615                                              pickup_goal.additional_link_padding) ) 
00616   {
00617     ROS_DEBUG_NAMED("manipulation","  Grasp executor: move_arm to pre-grasp reports failure");
00618     if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 1.0, 0.5, 0.0); //orange-ish
00619     return Result(GraspResult::MOVE_ARM_FAILED, true);
00620   }
00621 
00622   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00623                           object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP, -1);
00624   
00625   geometry_msgs::PoseStamped final_grasp_pose_stamped;
00626   final_grasp_pose_stamped.pose = grasp.grasp_pose;
00627   final_grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
00628   final_grasp_pose_stamped.header.stamp = ros::Time(0);
00629 
00630   object_manipulation_msgs::ReactiveGraspGoal reactive_grasp_goal;
00631   reactive_grasp_goal.arm_name = pickup_goal.arm_name;
00632   reactive_grasp_goal.target = pickup_goal.target;
00633   reactive_grasp_goal.final_grasp_pose = final_grasp_pose_stamped;
00634   reactive_grasp_goal.trajectory = execution_info.approach_trajectory_;
00635   reactive_grasp_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
00636   reactive_grasp_goal.pre_grasp_posture = grasp.pre_grasp_posture;
00637   reactive_grasp_goal.grasp_posture = grasp.grasp_posture;
00638 
00639   //give the reactive grasp 3 minutes to do its thing
00640   ros::Duration timeout = ros::Duration(180.0);
00641   mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_grasp_goal);
00642   if ( !mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
00643   {
00644     ROS_ERROR("  Reactive grasp timed out");
00645     return Result(GraspResult::GRASP_FAILED, false);
00646   }
00647   object_manipulation_msgs::ReactiveGraspResult reactive_grasp_result = 
00648     *mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).getResult();
00649   if (reactive_grasp_result.manipulation_result.value != reactive_grasp_result.manipulation_result.SUCCESS)
00650   {
00651     ROS_ERROR("Reactive grasp failed with error code %d", reactive_grasp_result.manipulation_result.value);
00652     return Result(GraspResult::GRASP_FAILED, false);
00653   }
00654   if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 0.0); //green
00655   return Result(GraspResult::SUCCESS, true);
00656 }
00657 
00658 object_manipulation_msgs::GraspResult 
00659 UnsafeGraspPerformer::approachAndGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00660                                        const object_manipulation_msgs::Grasp &grasp,
00661                                        GraspExecutionInfo &execution_info)
00662 {
00663   ROS_DEBUG_NAMED("manipulation", "executing unsafe grasp");
00664 
00665   //compute the pre-grasp pose
00666   //get the grasp pose in the right frame
00667   geometry_msgs::PoseStamped grasp_pose_stamped;
00668   grasp_pose_stamped.pose = grasp.grasp_pose;
00669   grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
00670   grasp_pose_stamped.header.stamp = ros::Time(0);
00671 
00672   //use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp
00673   geometry_msgs::Vector3Stamped direction;
00674   direction.header.stamp = ros::Time::now();
00675   direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
00676   direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
00677 
00678   //make sure the input is normalized
00679   geometry_msgs::Vector3Stamped direction_norm = direction;
00680   direction_norm.vector = mechInterface().normalize(direction.vector);
00681 
00682   //multiply the approach direction by the desired length and translate the grasp pose back along it
00683   double desired_trajectory_length = fabs(grasp.desired_approach_distance);
00684   direction_norm.vector.x *= desired_trajectory_length;
00685   direction_norm.vector.y *= desired_trajectory_length;
00686   direction_norm.vector.z *= desired_trajectory_length;
00687   geometry_msgs::PoseStamped pregrasp_pose = mechInterface().translateGripperPose(direction_norm, grasp_pose_stamped, 
00688                                                                                   pickup_goal.arm_name);
00689   
00690   //move to the pre-grasp using the Cartesian controller
00691   mechInterface().moveArmToPoseCartesian(pickup_goal.arm_name, pregrasp_pose, ros::Duration(15.0), 
00692                                          .0015, .01, 0.02, 0.16, 0.005, 0.087, 0.1, 
00693                                          execution_info.approach_trajectory_.points.front().positions);
00694 
00695   //move to the pre-grasp hand posture
00696   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00697                            object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP, -1);
00698 
00699   //if not using reactive grasping, execute the unnormalized interpolated trajectory from pre-grasp to grasp
00700   if(!pickup_goal.use_reactive_execution)
00701   {  
00702     mechInterface().attemptTrajectory(pickup_goal.arm_name, execution_info.approach_trajectory_, true);
00703     mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00704                            object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP, pickup_goal.max_contact_force);    
00705   }
00706 
00707   //otherwise, call reactive grasping
00708   else
00709   {
00710     geometry_msgs::PoseStamped final_grasp_pose_stamped;
00711     final_grasp_pose_stamped.pose = grasp.grasp_pose;
00712     final_grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
00713     final_grasp_pose_stamped.header.stamp = ros::Time(0);
00714 
00715     object_manipulation_msgs::ReactiveGraspGoal reactive_grasp_goal;
00716     reactive_grasp_goal.arm_name = pickup_goal.arm_name;
00717     reactive_grasp_goal.target = pickup_goal.target;
00718     reactive_grasp_goal.final_grasp_pose = final_grasp_pose_stamped;
00719     reactive_grasp_goal.trajectory = execution_info.approach_trajectory_;
00720     reactive_grasp_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
00721     reactive_grasp_goal.pre_grasp_posture = grasp.pre_grasp_posture;
00722     reactive_grasp_goal.grasp_posture = grasp.grasp_posture;
00723 
00724     //give the reactive grasp 3 minutes to do its thing
00725     ros::Duration timeout = ros::Duration(180.0);
00726     mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_grasp_goal);
00727     if ( !mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
00728     {
00729       ROS_ERROR("  Reactive grasp timed out");
00730       return Result(GraspResult::GRASP_FAILED, false);
00731     }
00732     object_manipulation_msgs::ReactiveGraspResult reactive_grasp_result = 
00733       *mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).getResult();
00734     if (reactive_grasp_result.manipulation_result.value != reactive_grasp_result.manipulation_result.SUCCESS)
00735     {
00736       ROS_ERROR("Reactive grasp failed with error code %d", reactive_grasp_result.manipulation_result.value);
00737       return Result(GraspResult::GRASP_FAILED, false);
00738     }
00739   }
00740   if (marker_publisher_) marker_publisher_->colorGraspMarker(execution_info.marker_id_, 0.0, 1.0, 0.0); //green
00741   return Result(GraspResult::SUCCESS, true);
00742 }
00743 
00744 }
approach_lift_grasp.cpp
