object_manipulator: grasp_executor_with

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_with_approach.h"
00037 
00038 #include "object_manipulator/tools/hand_description.h"
00039 #include "object_manipulator/tools/vector_tools.h"
00040 
00041 //#include <demo_synchronizer/synchronizer_client.h>
00042 
00043 using object_manipulation_msgs::GraspResult;
00044 using motion_planning_msgs::ArmNavigationErrorCodes;
00045 
00046 namespace object_manipulator {
00047 
00049 motion_planning_msgs::OrderedCollisionOperations 
00050 GraspExecutorWithApproach::collisionOperationsForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal)
00051 {
00052   motion_planning_msgs::OrderedCollisionOperations ord;
00053   motion_planning_msgs::CollisionOperation coll;
00054   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00055   coll.object2 = pickup_goal.collision_object_name;
00056   coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00057   ord.collision_operations.push_back(coll);
00058   if (pickup_goal.allow_gripper_support_collision)
00059   {
00060     coll.object2 = pickup_goal.collision_support_surface_name;
00061     ord.collision_operations.push_back(coll);
00062   }
00063   /*
00064   // not sure we need this anymore
00065   ROS_DEBUG("Disabling collisions between fingertips and table for grasp");
00066   for (size_t i=0; i<handDescription().fingertipLinks(pickup_goal.arm_name).size(); i++)
00067   {
00068     coll.object1 = pickup_goal.collision_support_surface_name;
00069     coll.object2 = handDescription().fingertipLinks(pickup_goal.arm_name).at(i);
00070     ord.collision_operations.push_back(coll);
00071   }
00072   */
00073   ord.collision_operations = concat(ord.collision_operations, 
00074                                     pickup_goal.additional_collision_operations.collision_operations);
00075   return ord;
00076 }
00077 
00079 std::vector<motion_planning_msgs::LinkPadding> 
00080 GraspExecutorWithApproach::linkPaddingForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal)
00081 {
00082   return concat(MechanismInterface::fingertipPadding(pickup_goal.arm_name, 0.0), 
00083                 pickup_goal.additional_link_padding);
00084 }
00085 
00086 GraspResult 
00087 GraspExecutorWithApproach::getInterpolatedIKForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00088                                                      const object_manipulation_msgs::Grasp &grasp,
00089                                                      trajectory_msgs::JointTrajectory &grasp_trajectory)
00090 {
00091   //get the grasp pose in the right frame
00092   geometry_msgs::PoseStamped grasp_pose_stamped;
00093   grasp_pose_stamped.pose = grasp.grasp_pose;
00094   grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
00095   grasp_pose_stamped.header.stamp = ros::Time(0);
00096 
00097   //use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp
00098   geometry_msgs::Vector3Stamped direction;
00099   direction.header.stamp = ros::Time::now();
00100   direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
00101   direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
00102 
00103   std::vector<double> empty;
00104   //remember to pass that we want to flip the trajectory
00105   float actual_approach_distance;
00106   int error_code = mechInterface().getInterpolatedIK(pickup_goal.arm_name, 
00107                                                      grasp_pose_stamped, 
00108                                                      direction, pickup_goal.desired_approach_distance,
00109                                                      empty, 
00110                                                      grasp.pre_grasp_posture,
00111                                                      collisionOperationsForGrasp(pickup_goal), 
00112                                                      linkPaddingForGrasp(pickup_goal),
00113                                                      true, grasp_trajectory, actual_approach_distance);
00114   ROS_DEBUG("  Grasp executor approach distance: actual (%f), min(%f) and desired (%f)", 
00115            actual_approach_distance, pickup_goal.min_approach_distance, pickup_goal.desired_approach_distance);
00116 
00117   if ( actual_approach_distance < pickup_goal.min_approach_distance)
00118   {
00119     ROS_DEBUG("  Grasp executor: interpolated IK for grasp below min threshold");
00120     if (grasp_trajectory.points.empty())
00121     {
00122       ROS_DEBUG("  Grasp executor: interpolaed IK empty, problem is with grasp location");
00123       if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 1.0, 0.0); //yellow
00124       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00125         return Result(GraspResult::GRASP_IN_COLLISION, true);
00126       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00127         return Result(GraspResult::GRASP_OUT_OF_REACH, true);
00128       else return Result(GraspResult::GRASP_UNFEASIBLE, true);      
00129     }
00130     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 1.0); //cyan
00131     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00132       return Result(GraspResult::PREGRASP_IN_COLLISION, true);
00133     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00134       return Result(GraspResult::PREGRASP_OUT_OF_REACH, true);
00135     else return Result(GraspResult::PREGRASP_UNFEASIBLE, true);      
00136   }
00137 
00138   //check if the first pose in trajectory is valid
00139   //when we check from pre-grasp to grasp we use custom link padding, so we need to check here
00140   //if the initial pose is feasible with default padding; otherwise, move_arm might refuse to 
00141   //take us there
00142   if ( !mechInterface().checkStateValidity(pickup_goal.arm_name, grasp_trajectory.points.front().positions,
00143                                            pickup_goal.additional_collision_operations,
00144                                            pickup_goal.additional_link_padding) )
00145   {
00146     ROS_DEBUG("  Grasp executor: initial pose in grasp trajectory is unfeasible with default padding");
00147     return Result(GraspResult::PREGRASP_UNFEASIBLE, true);      
00148   }
00149 
00150   return Result(GraspResult::SUCCESS, true);
00151 }
00152 
00153 
00154 GraspResult GraspExecutorWithApproach::prepareGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00155                                                     const object_manipulation_msgs::Grasp &grasp)
00156 {
00157   if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.0, 0.0); //red
00158 
00159   //demo_synchronizer::getClient().sync(3, "Computing interpolated IK path from pre-grasp to grasp to lift");
00160   //demo_synchronizer::getClient().rviz(3, "Collision models;Interpolated IK;IK contacts;Grasp execution");
00161   
00162   GraspResult result = getInterpolatedIKForGrasp(pickup_goal, grasp, interpolated_grasp_trajectory_);
00163   if ( result.result_code != GraspResult::SUCCESS )
00164   {
00165     ROS_DEBUG("  Grasp executor: failed to generate grasp trajectory");
00166     return result;
00167   }
00168 
00169   //check for lift trajectory starting from grasp solution
00170   std::vector<double> grasp_joint_angles = interpolated_grasp_trajectory_.points.back().positions;
00171   result = getInterpolatedIKForLift(pickup_goal, grasp, grasp_joint_angles, interpolated_lift_trajectory_);
00172   if (result.result_code != GraspResult::SUCCESS)
00173   {
00174     ROS_DEBUG("  Grasp executor: failed to generate lift trajectory");
00175     return result;
00176   }
00177 
00178   return Result(GraspResult::SUCCESS, true);
00179 }
00180 
00181 GraspResult 
00182 GraspExecutorWithApproach::executeGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00183                                         const object_manipulation_msgs::Grasp &grasp)
00184 {
00185   //demo_synchronizer::getClient().sync(2, "Using motion planner to move arm to pre-grasp position");
00186   //demo_synchronizer::getClient().rviz(1, "Collision models;Planning goal;Environment contacts;Collision map");
00187 
00188   if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name, 
00189                                              interpolated_grasp_trajectory_.points.front().positions,
00190                                              pickup_goal.additional_collision_operations,
00191                                              pickup_goal.additional_link_padding) ) 
00192   {
00193     ROS_DEBUG("  Grasp executor: move_arm to pre-grasp reports failure");
00194     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.5, 0.0); //orange-ish
00195     return Result(GraspResult::MOVE_ARM_FAILED, true);
00196   }
00197 
00198   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00199                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP);
00200 
00201   //demo_synchronizer::getClient().sync(2, "Executing interpolated IK path from pre-grasp to grasp");
00202   //demo_synchronizer::getClient().rviz(1, "Collision models");
00203 
00204   //execute the unnormalized interpolated trajectory from pre-grasp to grasp
00205   mechInterface().attemptTrajectory(pickup_goal.arm_name, interpolated_grasp_trajectory_, true);
00206 
00207   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00208                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP);    
00209 
00210   if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 0.0); //green
00211   return Result(GraspResult::SUCCESS, true);
00212 }
00213 
00218 GraspResult GraspExecutorWithApproach::retreat(const object_manipulation_msgs::PickupGoal &pickup_goal)
00219 {
00220   motion_planning_msgs::OrderedCollisionOperations ord;
00221   motion_planning_msgs::CollisionOperation coll;
00222   //disable collision between gripper and object
00223   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00224   coll.object2 = pickup_goal.collision_object_name;
00225   coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00226   ord.collision_operations.push_back(coll);
00227   //disable collision between gripper and table
00228   coll.object2 = pickup_goal.collision_support_surface_name;
00229   ord.collision_operations.push_back(coll);
00230   ord.collision_operations = concat(ord.collision_operations, 
00231                                     pickup_goal.additional_collision_operations.collision_operations);
00232 
00233   geometry_msgs::Vector3Stamped direction;
00234   direction.header.stamp = ros::Time::now();
00235   direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name);
00236   direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) );
00237 
00238   //even if the complete retreat trajectory is not possible, execute as many
00239   //steps as we can (pass min_distance = 0)
00240   float retreat_distance = pickup_goal.min_approach_distance;
00241   float actual_distance;
00242   if (!mechInterface().translateGripper(pickup_goal.arm_name, direction,
00243                                         ord, pickup_goal.additional_link_padding, 
00244                                         retreat_distance, 0, actual_distance))
00245   {
00246     ROS_ERROR(" Grasp executor: failed to retreat gripper at all");
00247     return Result(GraspResult::RETREAT_FAILED, false);
00248   }
00249   if (actual_distance < retreat_distance)
00250   {
00251     ROS_WARN(" Grasp executor: only partial retreat (%f) succeeeded", actual_distance);
00252     return Result(GraspResult::RETREAT_FAILED, false);
00253   }
00254   return Result(GraspResult::SUCCESS, true);
00255 }
00256 
00257 } //namespace object_manipulator
grasp_executor_with_approach.cpp
