katana_object_manipulator: katana_object_manipulator: reactive_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): Kaijen Hsiao Matei Ciocarlie
00035 
00036 #include "object_manipulator/grasp_execution/reactive_grasp_executor.h"
00037 
00038 #include <object_manipulation_msgs/ReactiveGrasp.h>
00039 
00040 //#include <demo_synchronizer/synchronizer_client.h>
00041 
00042 #include "object_manipulator/tools/hand_description.h"
00043 #include "object_manipulator/tools/vector_tools.h"
00044 
00045 using object_manipulation_msgs::GraspResult;
00046 
00047 namespace object_manipulator {
00048 
00049 GraspResult
00050 ReactiveGraspExecutor::executeGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal,
00051                                     const object_manipulation_msgs::Grasp &grasp)
00052 {
00053   //demo_synchronizer::getClient().sync(2, "Using motion planner to move arm to pre-grasp position");
00054   //demo_synchronizer::getClient().rviz(1, "Collision models;Environment contacts;Planning goal;Collision map");
00055 
00056   if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name,
00057                                              interpolated_grasp_trajectory_.points.front().positions,
00058                                              pickup_goal.additional_collision_operations,
00059                                              pickup_goal.additional_link_padding) )
00060   {
00061     ROS_DEBUG("  Grasp executor: move_arm to pre-grasp reports failure");
00062     if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.5, 0.0); //orange-ish
00063     return Result(GraspResult::MOVE_ARM_FAILED, true);
00064   }
00065 
00066   mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp,
00067                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP);
00068 
00069   geometry_msgs::PoseStamped final_grasp_pose_stamped;
00070   final_grasp_pose_stamped.pose = grasp.grasp_pose;
00071   final_grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id;
00072   final_grasp_pose_stamped.header.stamp = ros::Time(0);
00073 
00074   object_manipulation_msgs::ReactiveGraspGoal reactive_grasp_goal;
00075   reactive_grasp_goal.arm_name = pickup_goal.arm_name;
00076   reactive_grasp_goal.target = pickup_goal.target;
00077   reactive_grasp_goal.final_grasp_pose = final_grasp_pose_stamped;
00078   reactive_grasp_goal.trajectory = interpolated_grasp_trajectory_;
00079   reactive_grasp_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
00080   reactive_grasp_goal.pre_grasp_posture = grasp.pre_grasp_posture;
00081   reactive_grasp_goal.grasp_posture = grasp.grasp_posture;
00082 
00083   //demo_synchronizer::getClient().sync(2, "Calling fingertip reactive grasping module to execute grasp");
00084   //demo_synchronizer::getClient().rviz(1, "Collision models");
00085 
00086   //give the reactive grasp 3 minutes to do its thing
00087   ros::Duration timeout = ros::Duration(180.0);
00088   mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_grasp_goal);
00089   if ( !mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
00090   {
00091     ROS_ERROR("  Reactive grasp timed out");
00092     return Result(GraspResult::GRASP_FAILED, false);
00093   }
00094   object_manipulation_msgs::ReactiveGraspResult reactive_grasp_result =
00095     *mechInterface().reactive_grasp_action_client_.client(pickup_goal.arm_name).getResult();
00096   if (reactive_grasp_result.manipulation_result.value != reactive_grasp_result.manipulation_result.SUCCESS)
00097   {
00098     ROS_ERROR("Reactive grasp failed with error code %d", reactive_grasp_result.manipulation_result.value);
00099     return Result(GraspResult::GRASP_FAILED, false);
00100   }
00101   if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 0.0); //green
00102   return Result(GraspResult::SUCCESS, true);
00103 }
00104 
00105 GraspResult
00106 ReactiveGraspExecutor::nonReactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00107 {
00108   motion_planning_msgs::OrderedCollisionOperations ord;
00109   motion_planning_msgs::CollisionOperation coll;
00110   //disable collisions between end-effector and table
00111   coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name);
00112   coll.object2 = pickup_goal.collision_support_surface_name;
00113   coll.operation = motion_planning_msgs::CollisionOperation::DISABLE;
00114   ord.collision_operations.push_back(coll);
00115   //disable collisions between attached object and table
00116   coll.object1 = pickup_goal.collision_object_name;
00117   coll.object2 = pickup_goal.collision_support_surface_name;
00118   ord.collision_operations.push_back(coll);
00119   ord.collision_operations = concat(ord.collision_operations,
00120                                     pickup_goal.additional_collision_operations.collision_operations);
00121 
00122   float actual_distance;
00123   if (!mechInterface().translateGripper(pickup_goal.arm_name,
00124                                         pickup_goal.lift.direction,
00125                                         ord, pickup_goal.additional_link_padding,
00126                                         pickup_goal.lift.desired_distance, 0.0, actual_distance))
00127   {
00128     ROS_DEBUG("  Reactive grasp executor: lift performed no steps");
00129     return Result(GraspResult::LIFT_FAILED, false);
00130   }
00131   if (actual_distance < pickup_goal.lift.min_distance)
00132   {
00133     ROS_DEBUG("  Reactive grasp executor: lift distance below min threshold ");
00134     return Result(GraspResult::LIFT_FAILED, false);
00135   }
00136   if (actual_distance < pickup_goal.lift.desired_distance)
00137   {
00138     ROS_DEBUG("  Reactive grasp executor: lift distance below desired, but above min threshold");
00139   }
00140   else
00141   {
00142     ROS_DEBUG("  Reactive grasp executor: desired lift distance executed");
00143   }
00144   return Result(GraspResult::SUCCESS, true);
00145 }
00146 
00147 GraspResult
00148 ReactiveGraspExecutor::reactiveLift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00149 {
00150   object_manipulation_msgs::ReactiveLiftGoal reactive_lift_goal;
00151   reactive_lift_goal.lift = pickup_goal.lift;
00152   reactive_lift_goal.arm_name = pickup_goal.arm_name;
00153   reactive_lift_goal.target = pickup_goal.target;
00154   reactive_lift_goal.trajectory = interpolated_lift_trajectory_;
00155   reactive_lift_goal.collision_support_surface_name = pickup_goal.collision_support_surface_name;
00156 
00157   //demo_synchronizer::getClient().sync(2, "Calling fingertip reactive grasping module to execute lift");
00158   //demo_synchronizer::getClient().rviz(1, "Collision models");
00159 
00160   //give the reactive lift 1 minute to do its thing
00161   ros::Duration timeout = ros::Duration(60.0);
00162   mechInterface().reactive_lift_action_client_.client(pickup_goal.arm_name).sendGoal(reactive_lift_goal);
00163   if ( !mechInterface().reactive_lift_action_client_.client(pickup_goal.arm_name).waitForResult(timeout) )
00164   {
00165     ROS_ERROR("  Reactive lift timed out");
00166     return Result(GraspResult::LIFT_FAILED, false);
00167   }
00168   object_manipulation_msgs::ReactiveLiftResult reactive_lift_result =
00169     *mechInterface().reactive_lift_action_client_.client(pickup_goal.arm_name).getResult();
00170   if (reactive_lift_result.manipulation_result.value != reactive_lift_result.manipulation_result.SUCCESS)
00171   {
00172     ROS_ERROR("Reactive lift failed with error code %d", reactive_lift_result.manipulation_result.value);
00173     return Result(GraspResult::LIFT_FAILED, false);
00174   }
00175   return Result(GraspResult::SUCCESS, true);
00176 }
00177 
00178 GraspResult
00179 ReactiveGraspExecutor::lift(const object_manipulation_msgs::PickupGoal &pickup_goal)
00180 {
00181   if (pickup_goal.use_reactive_lift)
00182   {
00183     return reactiveLift(pickup_goal);
00184   }
00185   else
00186   {
00187     return nonReactiveLift(pickup_goal);
00188   }
00189 }
00190 
00191 } //namespace object_manipulator
reactive_grasp_executor.cpp
