$search
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 ROS_INFO("Setting FingertipPadding to zero"); 00083 return concat(MechanismInterface::fingertipPadding(pickup_goal.arm_name, 0.0), 00084 pickup_goal.additional_link_padding); 00085 } 00086 00087 GraspResult GraspExecutorWithApproach::getInterpolatedIKForGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal, 00088 const object_manipulation_msgs::Grasp &grasp, 00089 trajectory_msgs::JointTrajectory &grasp_trajectory) 00090 { 00091 ROS_INFO("Start getIIK4Grasp.."); 00092 //get the grasp pose in the right frame 00093 geometry_msgs::PoseStamped grasp_pose_stamped; 00094 grasp_pose_stamped.pose = grasp.grasp_pose; 00095 grasp_pose_stamped.header.frame_id = pickup_goal.target.reference_frame_id; 00096 grasp_pose_stamped.header.stamp = ros::Time(0); 00097 00098 ROS_INFO("Executor_getIIK4Grasp: Pose %f %f %f in Frame: %s", grasp_pose_stamped.pose.position.x, grasp_pose_stamped.pose.position.y, grasp_pose_stamped.pose.position.z,pickup_goal.target.reference_frame_id.c_str() ); 00099 00100 //use the opposite of the approach direction as we are going backwards, from grasp to pre-grasp 00101 geometry_msgs::Vector3Stamped direction; 00102 direction.header.stamp = ros::Time::now(); 00103 direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name); 00104 direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) ); 00105 00106 ROS_INFO("Executor_getIIK4Grasp: DirectionVector %f %f %f", direction.vector.x, direction.vector.y, direction.vector.z); 00107 ROS_INFO("Executor_getIIK4Grasp: Distance %f", pickup_goal.desired_approach_distance); 00108 00109 std::vector<double> empty; 00110 //remember to pass that we want to flip the trajectory 00111 float actual_approach_distance; 00112 int error_code = mechInterface().getInterpolatedIK(pickup_goal.arm_name, 00113 grasp_pose_stamped, 00114 direction, pickup_goal.desired_approach_distance, 00115 empty, 00116 grasp.pre_grasp_posture, 00117 collisionOperationsForGrasp(pickup_goal), 00118 linkPaddingForGrasp(pickup_goal), 00119 true, grasp_trajectory, actual_approach_distance); 00120 ROS_DEBUG(" Grasp executor approach distance: actual (%f), min(%f) and desired (%f)", 00121 actual_approach_distance, pickup_goal.min_approach_distance, pickup_goal.desired_approach_distance); 00122 00123 if ( actual_approach_distance < pickup_goal.min_approach_distance) 00124 { 00125 ROS_DEBUG(" Grasp executor: interpolated IK for grasp below min threshold"); 00126 if (grasp_trajectory.points.empty()) 00127 { 00128 ROS_DEBUG(" Grasp executor: interpolated IK empty, problem is with grasp location"); 00129 if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 1.0, 0.0); //yellow 00130 if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 00131 return Result(GraspResult::GRASP_IN_COLLISION, true); 00132 else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) 00133 return Result(GraspResult::GRASP_OUT_OF_REACH, true); 00134 else return Result(GraspResult::GRASP_UNFEASIBLE, true); 00135 } 00136 if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 1.0); //cyan 00137 if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 00138 return Result(GraspResult::PREGRASP_IN_COLLISION, true); 00139 else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) 00140 return Result(GraspResult::PREGRASP_OUT_OF_REACH, true); 00141 else return Result(GraspResult::PREGRASP_UNFEASIBLE, true); 00142 } 00143 00144 //check if the first pose in trajectory is valid 00145 //when we check from pre-grasp to grasp we use custom link padding, so we need to check here 00146 //if the initial pose is feasible with default padding; otherwise, move_arm might refuse to 00147 //take us there 00148 if ( !mechInterface().checkStateValidity(pickup_goal.arm_name, grasp_trajectory.points.front().positions, 00149 pickup_goal.additional_collision_operations, 00150 pickup_goal.additional_link_padding) ) 00151 { 00152 ROS_DEBUG(" Grasp executor: initial pose in grasp trajectory is unfeasible with default padding"); 00153 return Result(GraspResult::PREGRASP_UNFEASIBLE, true); 00154 } 00155 00156 return Result(GraspResult::SUCCESS, true); 00157 } 00158 00159 00160 GraspResult GraspExecutorWithApproach::prepareGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal, 00161 const object_manipulation_msgs::Grasp &grasp) 00162 { 00163 if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.0, 0.0); //red 00164 00165 //demo_synchronizer::getClient().sync(3, "Computing interpolated IK path from pre-grasp to grasp to lift"); 00166 //demo_synchronizer::getClient().rviz(3, "Collision models;Interpolated IK;IK contacts;Grasp execution"); 00167 00168 GraspResult result = getInterpolatedIKForGrasp(pickup_goal, grasp, interpolated_grasp_trajectory_); 00169 if ( result.result_code != GraspResult::SUCCESS ) 00170 { 00171 ROS_DEBUG(" Grasp executor: failed to generate grasp trajectory"); 00172 return result; 00173 } 00174 00175 ROS_INFO(" Grasp executor: sucessfully computed IIK trac to grasp pose"); 00176 00177 //check for lift trajectory starting from grasp solution 00178 std::vector<double> grasp_joint_angles = interpolated_grasp_trajectory_.points.back().positions; 00179 result = getInterpolatedIKForLift(pickup_goal, grasp, grasp_joint_angles, interpolated_lift_trajectory_); 00180 if (result.result_code != GraspResult::SUCCESS) 00181 { 00182 ROS_DEBUG(" Grasp executor: failed to generate lift trajectory"); 00183 return result; 00184 } 00185 00186 ROS_INFO(" Grasp executor: sucessfully computed IIK to lift pose"); 00187 00188 return Result(GraspResult::SUCCESS, true); 00189 } 00190 00191 GraspResult 00192 GraspExecutorWithApproach::executeGrasp(const object_manipulation_msgs::PickupGoal &pickup_goal, 00193 const object_manipulation_msgs::Grasp &grasp) 00194 { 00195 //demo_synchronizer::getClient().sync(2, "Using motion planner to move arm to pre-grasp position"); 00196 //demo_synchronizer::getClient().rviz(1, "Collision models;Planning goal;Environment contacts;Collision map"); 00197 00198 ROS_INFO(" Grasp executor: move to pre-grasp pose"); 00199 if ( !mechInterface().attemptMoveArmToGoal(pickup_goal.arm_name, 00200 interpolated_grasp_trajectory_.points.front().positions, 00201 pickup_goal.additional_collision_operations, 00202 pickup_goal.additional_link_padding) ) 00203 { 00204 ROS_DEBUG(" Grasp executor: move_arm to pre-grasp reports failure"); 00205 if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.5, 0.0); //orange-ish 00206 return Result(GraspResult::MOVE_ARM_FAILED, true); 00207 } 00208 ROS_INFO(" Grasp executor: move hand to pregrasp posture"); 00209 mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 00210 object_manipulation_msgs::GraspHandPostureExecutionGoal::PRE_GRASP); 00211 00212 //demo_synchronizer::getClient().sync(2, "Executing interpolated IK path from pre-grasp to grasp"); 00213 //demo_synchronizer::getClient().rviz(1, "Collision models"); 00214 00215 ROS_INFO(" Grasp executor: attempt iik trajactory"); 00216 //execute the unnormalized interpolated trajectory from pre-grasp to grasp 00217 mechInterface().attemptTrajectory(pickup_goal.arm_name, interpolated_grasp_trajectory_, true); 00218 00219 ROS_INFO(" Grasp executor: move hand to grasp posture"); 00220 mechInterface().handPostureGraspAction(pickup_goal.arm_name, grasp, 00221 object_manipulation_msgs::GraspHandPostureExecutionGoal::GRASP); 00222 00223 if (marker_publisher_) marker_publisher_->colorGraspMarker(marker_id_, 0.0, 1.0, 0.0); //green 00224 return Result(GraspResult::SUCCESS, true); 00225 } 00226 00231 GraspResult GraspExecutorWithApproach::retreat(const object_manipulation_msgs::PickupGoal &pickup_goal) 00232 { 00233 motion_planning_msgs::OrderedCollisionOperations ord; 00234 motion_planning_msgs::CollisionOperation coll; 00235 //disable collision between gripper and object 00236 coll.object1 = handDescription().gripperCollisionName(pickup_goal.arm_name); 00237 coll.object2 = pickup_goal.collision_object_name; 00238 coll.operation = motion_planning_msgs::CollisionOperation::DISABLE; 00239 ord.collision_operations.push_back(coll); 00240 //disable collision between gripper and table 00241 coll.object2 = pickup_goal.collision_support_surface_name; 00242 ord.collision_operations.push_back(coll); 00243 ord.collision_operations = concat(ord.collision_operations, 00244 pickup_goal.additional_collision_operations.collision_operations); 00245 00246 geometry_msgs::Vector3Stamped direction; 00247 direction.header.stamp = ros::Time::now(); 00248 direction.header.frame_id = handDescription().gripperFrame(pickup_goal.arm_name); 00249 direction.vector = mechInterface().negate( handDescription().approachDirection(pickup_goal.arm_name) ); 00250 00251 //even if the complete retreat trajectory is not possible, execute as many 00252 //steps as we can (pass min_distance = 0) 00253 float retreat_distance = pickup_goal.min_approach_distance; 00254 float actual_distance; 00255 if (!mechInterface().translateGripper(pickup_goal.arm_name, direction, 00256 ord, pickup_goal.additional_link_padding, 00257 retreat_distance, 0, actual_distance)) 00258 { 00259 ROS_ERROR(" Grasp executor: failed to retreat gripper at all"); 00260 return Result(GraspResult::RETREAT_FAILED, false); 00261 } 00262 if (actual_distance < retreat_distance) 00263 { 00264 ROS_WARN(" Grasp executor: only partial retreat (%f) succeeeded", actual_distance); 00265 return Result(GraspResult::RETREAT_FAILED, false); 00266 } 00267 return Result(GraspResult::SUCCESS, true); 00268 } 00269 00270 } //namespace object_manipulator