object_manipulator: grasp_executor.cpp Source File

00001 /*********************************************************************
00002 *
00003 *  Copyright (c) 2009, Willow Garage, Inc.
00004 *  All rights reserved.
00005 *
00006 *  Redistribution and use in source and binary forms, with or without
00007 *  modification, are permitted provided that the following conditions
00008 *  are met:
00009 *
00010 *   * Redistributions of source code must retain the above copyright
00011 *     notice, this list of conditions and the following disclaimer.
00012 *   * Redistributions in binary form must reproduce the above
00013 *     copyright notice, this list of conditions and the following
00014 *     disclaimer in the documentation and/or other materials provided
00015 *     with the distribution.
00016 *   * Neither the name of the Willow Garage nor the names of its
00017 *     contributors may be used to endorse or promote products derived
00018 *     from this software without specific prior written permission.
00019 *
00020 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 *  POSSIBILITY OF SUCH DAMAGE.
00032 *********************************************************************/
00033 
00034 // Author(s): Matei Ciocarlie
00035 
00036 #include "object_manipulator/grasp_execution/grasp_executor.h"
00037 
00038 #include "object_manipulator/tools/exceptions.h"
00039 #include "object_manipulator/tools/hand_description.h"
00040 #include "object_manipulator/tools/vector_tools.h"
00041 
00042 using object_manipulation_msgs::GraspResult;
00043 using motion_planning_msgs::ArmNavigationErrorCodes;
00044 
00045 namespace object_manipulator {
00046 
00048 motion_planning_msgs::OrderedCollisionOperations 
00049 GraspExecutor::collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00050 {
00051   motion_planning_msgs::OrderedCollisionOperations ord;
00052   motion_planning_msgs::CollisionOperation coll;
00053   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00054   coll.object2 = pickup_goal.collision_object_name;
00055   coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00056   ord.collision_operations.push_back(coll);
00057   if (pickup_goal.allow_gripper_support_collision)
00058   {
00059     coll.object2 = pickup_goal.collision_support_surface_name;
00060     ord.collision_operations.push_back(coll);
00061   }
00062   ord.collision_operations = concat(ord.collision_operations, 
00063                                     pickup_goal.additional_collision_operations.collision_operations);
00064   return ord;
00065 }
00066 
00068 std::vector<motion_planning_msgs::LinkPadding> 
00069 GraspExecutor::linkPaddingForLift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00070 {
00071   return concat(MechanismInterface::fingertipPadding(pickup_goal.arm_name, 0.0), 
00072                 pickup_goal.additional_link_padding);
00073 }
00074 
00075 GraspResult 
00076 GraspExecutor::getInterpolatedIKForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00077                                         const object_manipulation_msgs::Grasp &grasp,
00078                                         const std::vector<double> &grasp_joint_angles,
00079                                         trajectory_msgs::JointTrajectory &lift_trajectory)
00080 {
00081   geometry_msgs::PoseStamped grasp_pose;
00082   grasp_pose.pose = grasp.grasp_pose;
00083   grasp_pose.header.frame_id = pickup_goal.target.reference_frame_id;
00084   grasp_pose.header.stamp = ros::Time(0);
00085 
00086   float actual_lift_distance;
00087   int error_code = 
00088     mechInterface().getInterpolatedIK(pickup_goal.arm_name, 
00089                                       grasp_pose, 
00090                                       pickup_goal.lift.direction, pickup_goal.lift.desired_distance,
00091                                       grasp_joint_angles, 
00092                                       grasp.grasp_posture, 
00093                                       collisionOperationsForLift(pickup_goal), linkPaddingForLift(pickup_goal),
00094                                       false, lift_trajectory, actual_lift_distance);
00095   ROS_DEBUG("  Lift distance: actual %f, min %f and desired %f", actual_lift_distance, pickup_goal.lift.min_distance,
00096            pickup_goal.lift.desired_distance);
00097 
00098   if (actual_lift_distance < pickup_goal.lift.min_distance)
00099   {
00100     ROS_DEBUG("  Lift trajectory  below min. threshold");
00101     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 0.0, 1.0); //blue
00102     if (lift_trajectory.points.empty())
00103     {
00104       ROS_DEBUG("  Lift trajectory empty; problem is with grasp location");
00105       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00106         return Result(GraspResult::GRASP_IN_COLLISION, true);
00107       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00108         return Result(GraspResult::GRASP_OUT_OF_REACH, true);
00109       else return Result(GraspResult::GRASP_UNFEASIBLE, true);
00110     }
00111     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00112       return Result(GraspResult::LIFT_IN_COLLISION, true);
00113     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00114       return Result(GraspResult::LIFT_OUT_OF_REACH, true);
00115     else return Result(GraspResult::LIFT_UNFEASIBLE, true);
00116   }
00117   
00118   //check if the last pose in trajectory is valid
00119   //when we check for lift we use custom link padding, so we need to check here if the last pose 
00120   //is feasible with default padding; otherwise, move_arm might refuse to take us out of there
00121   if ( pickup_goal.lift.min_distance != 0 && 
00122        !mechInterface().checkStateValidity(pickup_goal.arm_name, lift_trajectory.points.back().positions,
00123                                            collisionOperationsForLift(pickup_goal),
00124                                            pickup_goal.additional_link_padding) )
00125   {
00126     ROS_DEBUG("  Grasp executor: last pose in lift trajectory is unfeasible with default padding");
00127     return Result(GraspResult::LIFT_UNFEASIBLE, true);
00128   }
00129 
00130   return Result(GraspResult::SUCCESS, true);
00131 }
00132 
00133 GraspResult 
00134 GraspExecutor::lift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00135 {
00136   if (interpolated_lift_trajectory_.points.empty())
00137   {
00138     ROS_ERROR("  Grasp executor: lift trajectory not set");
00139     return Result(GraspResult::LIFT_FAILED, false);
00140   }
00141 
00142   //execute the unnormalized interpolated trajectory from grasp to lift
00143   mechInterface().attemptTrajectory(pickup_goal.arm_name, interpolated_lift_trajectory_, true);
00144   return Result(GraspResult::SUCCESS, true);
00145 }
00146 
00147 GraspResult 
00148 GraspExecutor::checkAndExecuteGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00149                                     const object_manipulation_msgs::Grasp &grasp)
00150 {
00151   //test if grasp is feasible and prepare trajectories  
00152   if (marker_publisher_)
00153   {
00154     geometry_msgs::PoseStamped marker_pose;
00155     marker_pose.pose = grasp.grasp_pose;
00156     marker_pose.header.frame_id = pickup_goal.target.reference_frame_id;
00157     marker_pose.header.stamp = ros::Time::now();
00158     marker_id_ = marker_publisher_->addGraspMarker(marker_pose);
00159   }  
00160   GraspResult result = prepareGrasp(pickup_goal, grasp);
00161   if (result.result_code != GraspResult::SUCCESS) return result;
00162 
00163   result = executeGrasp(pickup_goal, grasp);
00164   if (result.result_code != GraspResult::SUCCESS) return result;
00165 
00166   //check if there is anything in gripper; if not, open gripper and retreat
00167   if (!mechInterface().graspPostureQuery(pickup_goal.arm_name))
00168   {
00169     ROS_DEBUG("Hand reports that grasp was not successfully executed; releasing object and retreating");
00170     mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
00171                                            object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE);    
00172     retreat(pickup_goal);
00173     return Result(GraspResult::GRASP_FAILED, false);
00174   }
00175   
00176   //attach object to gripper
00177   if (!pickup_goal.collision_object_name.empty())
00178   {
00179     mechInterface().attachObjectToGripper(pickup_goal.arm_name, pickup_goal.collision_object_name);
00180   }
00181 
00182   //lift the object
00183   result = lift(pickup_goal);
00184   if (result.result_code != GraspResult::SUCCESS) return result;
00185 
00186   return Result(GraspResult::SUCCESS, true);
00187 }
00188 
00189 } //namespace object_manipulator