object_manipulator: simple_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/simple_grasp_executor.h"
00037 
00038 //#include <demo_synchronizer/synchronizer_client.h>
00039 
00040 using object_manipulation_msgs::GraspResult;
00041 
00042 namespace object_manipulator {
00043 
00044 GraspResult SimpleGraspExecutor::prepareGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00045                                               const object_manipulation_msgs::Grasp &grasp)
00046 {
00047   if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.0, 0.0); //red
00048 
00049   geometry_msgs::PoseStamped grasp_pose;
00050   grasp_pose.pose = grasp.grasp_pose;
00051   grasp_pose.header.frame_id = pickup_goal.target.reference_frame_id;
00052   grasp_pose.header.stamp = ros::Time(0);
00053 
00054   //demo_synchronizer::getClient().sync(3, "Checking IK solution for final grasp position and interpolated IK for lift");
00055   //demo_synchronizer::getClient().rviz(3, "Collision models;Interpolated IK;IK contacts;Grasp execution");
00056 
00057   if ( !mechInterface().getIKForPose(pickup_goal.arm_name, grasp_pose, ik_response_,
00058                                      pickup_goal.additional_collision_operations,
00059                                      pickup_goal.additional_link_padding) ) 
00060   {
00061     ROS_DEBUG_STREAM("  Grasp execution: initial IK check failed");
00062     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 1.0, 0.0); //yellow
00063     return Result(GraspResult::GRASP_UNFEASIBLE, true);
00064   }
00065   ROS_DEBUG("  Grasp executor: IK for grasp succeeded.");
00066 
00067   std::vector<double> grasp_joint_angles = ik_response_.solution.joint_state.position;
00068   GraspResult result = getInterpolatedIKForLift(pickup_goal, grasp, grasp_joint_angles, interpolated_lift_trajectory_);
00069   if (result.result_code != GraspResult::SUCCESS) return result;
00070 
00071   ROS_DEBUG("  Grasp executor: interpolated IK for lift succeeded.");  
00072   return Result(GraspResult::SUCCESS, true);
00073 }
00074 
00075 GraspResult 
00076 SimpleGraspExecutor::executeGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00077                                   const object_manipulation_msgs::Grasp &grasp)
00078 {
00079   //demo_synchronizer::getClient().sync(2, "Using motion planner to move the arm to grasp position");
00080   //demo_synchronizer::getClient().rviz(1, "Collision models;Planning goal;Collision map;Environment contacts");
00081 
00082   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00083                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP);
00084 
00085   if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name, ik_response_.solution.joint_state.position,
00086                                              pickup_goal.additional_collision_operations,
00087                                              pickup_goal.additional_link_padding) ) 
00088   {
00089     ROS_DEBUG("  Grasp execution: move arm reports failure");
00090     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.5, 0.0); //orange-ish
00091     return Result(GraspResult::MOVE_ARM_FAILED, true);
00092   }
00093 
00094   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 
00095                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP);    
00096   if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 0.0); //green
00097   return Result(GraspResult::SUCCESS, true);
00098 }
00099 
00100 } //namespace object_manipulator