object_manipulator: object_manipulator: grasp

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 arm_navigation_msgs::ArmNavigationErrorCodes;
00044 
00045 namespace object_manipulator {
00046 
00048 arm_navigation_msgs::OrderedCollisionOperations 
00049 GraspExecutor::collisionOperationsForLift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00050 {
00051   arm_navigation_msgs::OrderedCollisionOperations ord;
00052   arm_navigation_msgs::CollisionOperation coll;
00053   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00054   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00055   if (!pickup_goal.collision_object_name.empty())
00056   {
00057     coll.object2 = pickup_goal.collision_object_name;
00058     ord.collision_operations.push_back(coll);
00059   }
00060   if (pickup_goal.allow_gripper_support_collision)
00061   {
00062     coll.object2 = pickup_goal.collision_support_surface_name;
00063     ord.collision_operations.push_back(coll);
00064   }
00065   ord.collision_operations = concat(ord.collision_operations, 
00066                                     pickup_goal.additional_collision_operations.collision_operations);
00067   return ord;
00068 }
00069 
00071 std::vector<arm_navigation_msgs::LinkPadding> 
00072 GraspExecutor::linkPaddingForLift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00073 {
00074   //return concat(MechanismInterface::fingertipPadding(pickup_goal.arm_name, 0.0), 
00075   //              pickup_goal.additional_link_padding);
00076   return concat(MechanismInterface::gripperPadding(pickup_goal.arm_name, 0.0), 
00077                 pickup_goal.additional_link_padding);
00078 }
00079 
00080 GraspResult 
00081 GraspExecutor::getInterpolatedIKForLift(const object_manipulation_msgs::PickupGoal &pickup_goal,
00082                                         const object_manipulation_msgs::Grasp &grasp,
00083                                         const std::vector<double> &grasp_joint_angles,
00084                                         trajectory_msgs::JointTrajectory &lift_trajectory)
00085 {
00086   geometry_msgs::PoseStamped grasp_pose;
00087   grasp_pose.pose = grasp.grasp_pose;
00088   grasp_pose.header.frame_id = pickup_goal.target.reference_frame_id;
00089   grasp_pose.header.stamp = ros::Time(0);
00090 
00091   float actual_lift_distance;
00092   int error_code = 
00093     mechInterface().getInterpolatedIK(pickup_goal.arm_name, 
00094                                       grasp_pose, 
00095                                       pickup_goal.lift.direction, pickup_goal.lift.desired_distance,
00096                                       grasp_joint_angles, 
00097                                       grasp.grasp_posture, 
00098                                       collisionOperationsForLift(pickup_goal), linkPaddingForLift(pickup_goal),
00099                                       false, lift_trajectory, actual_lift_distance);
00100   ROS_DEBUG_NAMED("manipulation","  Lift distance: actual %f, min %f and desired %f", actual_lift_distance, pickup_goal.lift.min_distance,
00101            pickup_goal.lift.desired_distance);
00102 
00103   if (actual_lift_distance < pickup_goal.lift.min_distance)
00104   {
00105     ROS_DEBUG_NAMED("manipulation","  Lift trajectory  below min. threshold");
00106     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 0.0, 1.0); //blue
00107     if (lift_trajectory.points.empty())
00108     {
00109       ROS_DEBUG_NAMED("manipulation","  Lift trajectory empty; problem is with grasp location");
00110       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00111         return Result(GraspResult::GRASP_IN_COLLISION, true);
00112       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00113         return Result(GraspResult::GRASP_OUT_OF_REACH, true);
00114       else return Result(GraspResult::GRASP_UNFEASIBLE, true);
00115     }
00116     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00117       return Result(GraspResult::LIFT_IN_COLLISION, true);
00118     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00119       return Result(GraspResult::LIFT_OUT_OF_REACH, true);
00120     else return Result(GraspResult::LIFT_UNFEASIBLE, true);
00121   }
00122 
00123   // note: here we used to check if the last state is valid with default padding. That
00124   // check has been moved to prepareGrasp() so that it can be grouped together with other checks that
00125   // use similar planning scenes.
00126   
00127   return Result(GraspResult::SUCCESS, true);
00128 }
00129 
00130 GraspResult 
00131 GraspExecutor::lift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00132 {
00133   if (interpolated_lift_trajectory_.points.empty())
00134   {
00135     ROS_ERROR("  Grasp executor: lift trajectory not set");
00136     return Result(GraspResult::LIFT_FAILED, false);
00137   }
00138 
00139   //execute the unnormalized interpolated trajectory from grasp to lift
00140   mechInterface().attemptTrajectory(pickup_goal.arm_name, interpolated_lift_trajectory_, true);
00141   return Result(GraspResult::SUCCESS, true);
00142 }
00143 
00144 GraspResult 
00145 GraspExecutor::checkAndExecuteGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00146                                     const object_manipulation_msgs::Grasp &grasp)
00147 {
00148   //test if grasp is feasible and prepare trajectories  
00149   if (marker_publisher_)
00150   {
00151     geometry_msgs::PoseStamped marker_pose;
00152     marker_pose.pose = grasp.grasp_pose;
00153     marker_pose.header.frame_id = pickup_goal.target.reference_frame_id;
00154     marker_pose.header.stamp = ros::Time::now();
00155     marker_id_ = marker_publisher_->addGraspMarker(marker_pose);
00156   }  
00157   GraspResult result = prepareGrasp(pickup_goal, grasp);
00158   if (result.result_code != GraspResult::SUCCESS || pickup_goal.only_perform_feasibility_test) return result;
00159 
00160   result = executeGrasp(pickup_goal, grasp);
00161   if (result.result_code != GraspResult::SUCCESS) return result;
00162 
00163   //check if there is anything in gripper; if not, open gripper and retreat
00164   if (!mechInterface().graspPostureQuery(pickup_goal.arm_name, grasp))
00165   {
00166     ROS_DEBUG_NAMED("manipulation","Hand reports that grasp was not successfully executed; "
00167                     "releasing object and retreating");
00168     mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
00169                                     object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, -1);    
00170     retreat(pickup_goal, grasp);
00171     return Result(GraspResult::GRASP_FAILED, false);
00172   }
00173   
00174   //attach object to gripper
00175   if (!pickup_goal.collision_object_name.empty())
00176   {
00177     mechInterface().attachObjectToGripper(pickup_goal.arm_name, pickup_goal.collision_object_name);
00178   }
00179 
00180   //lift the object
00181   result = lift(pickup_goal);
00182   if (result.result_code != GraspResult::SUCCESS) return result;
00183 
00184   return Result(GraspResult::SUCCESS, true);
00185 }
00186 
00187 } //namespace object_manipulator
grasp_executor.cpp
