object_manipulator: object_manipulator: place

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 #include "object_manipulator/place_execution/place_executor.h"
00035 
00036 #include "object_manipulator/tools/hand_description.h"
00037 #include "object_manipulator/tools/exceptions.h"
00038 #include "object_manipulator/tools/vector_tools.h"
00039 
00040 //#include <demo_synchronizer/synchronizer_client.h>
00041 
00042 using object_manipulation_msgs::PlaceLocationResult;
00043 using arm_navigation_msgs::ArmNavigationErrorCodes;
00044 
00045 namespace object_manipulator {
00046 
00047 geometry_msgs::PoseStamped PlaceExecutor::computeGripperPose(geometry_msgs::PoseStamped place_location, 
00048                                                              geometry_msgs::Pose grasp_pose, 
00049                                                              std::string frame_id)
00050 {
00051   //get the gripper pose relative to place location
00052   tf::Transform place_trans;
00053   tf::poseMsgToTF(place_location.pose, place_trans);
00054   tf::Transform grasp_trans;
00055   tf::poseMsgToTF(grasp_pose, grasp_trans);
00056   grasp_trans = place_trans * grasp_trans;
00057 
00058   //get it in the requested frame
00059   tf::Stamped<tf::Pose> grasp_trans_stamped;
00060   grasp_trans_stamped.setData(grasp_trans);
00061   grasp_trans_stamped.frame_id_ = place_location.header.frame_id;
00062   grasp_trans_stamped.stamp_ = ros::Time::now();
00063   if (!listener_.waitForTransform(frame_id, place_location.header.frame_id, ros::Time::now(), ros::Duration(1.0)))
00064   {
00065     ROS_ERROR("Object place: tf does not have transform from %s to %s", 
00066               place_location.header.frame_id.c_str(),
00067               frame_id.c_str());
00068     throw MechanismException( std::string("Object place: tf does not have transform from ") + 
00069                               place_location.header.frame_id.c_str() + std::string(" to ") + 
00070                               frame_id);
00071   }
00072   tf::Stamped<tf::Pose> grasp_trans_base;
00073   try
00074   {
00075     listener_.transformPose( frame_id, grasp_trans_stamped, grasp_trans_base);
00076   }
00077   catch (tf::TransformException ex)
00078   {
00079     ROS_ERROR("Object place: tf failed to transform gripper pose into frame %s; exception: %s", 
00080               frame_id.c_str(), ex.what());
00081     throw MechanismException( std::string("tf failed to transform gripper pose into frame ") + 
00082                               frame_id + std::string("; tf exception: ") +  
00083                               std::string(ex.what()) );
00084   }
00085   
00086   geometry_msgs::PoseStamped gripper_pose;
00087   tf::poseTFToMsg(grasp_trans_base, gripper_pose.pose);
00088   gripper_pose.header.frame_id = frame_id;
00089   gripper_pose.header.stamp = ros::Time::now();
00090   return gripper_pose;
00091 }
00092 
00107 PlaceLocationResult PlaceExecutor::retreat(const object_manipulation_msgs::PlaceGoal &place_goal)
00108 {
00109   arm_navigation_msgs::OrderedCollisionOperations ord;
00110   arm_navigation_msgs::CollisionOperation coll;
00111   coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00112   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00113   //disable collision between gripper and object
00114   if (!place_goal.collision_object_name.empty())
00115   {
00116     coll.object2 = place_goal.collision_object_name;
00117     ord.collision_operations.push_back(coll);
00118   }
00119   //disable collision between gripper and table
00120   if (!place_goal.collision_support_surface_name.empty()) 
00121   {
00122     coll.object2 = place_goal.collision_support_surface_name;
00123     ord.collision_operations.push_back(coll);
00124   }
00125   ord.collision_operations = concat(ord.collision_operations, 
00126                                     place_goal.additional_collision_operations.collision_operations);
00127 
00128   //zero padding on fingertip links; shouldn't matter much due to collisions disabled above
00129   //std::vector<arm_navigation_msgs::LinkPadding> link_padding 
00130   //  = concat(MechanismInterface::fingertipPadding(place_goal.arm_name, 0.0),
00131   //           place_goal.additional_link_padding);
00132   std::vector<arm_navigation_msgs::LinkPadding> link_padding 
00133     = concat(MechanismInterface::gripperPadding(place_goal.arm_name, 0.0),
00134              place_goal.additional_link_padding);
00135 
00136   geometry_msgs::Vector3Stamped direction;
00137   direction.header.stamp = ros::Time::now();
00138   direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00139   direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00140 
00141   float actual_distance;
00142   mechInterface().translateGripper(place_goal.arm_name, 
00143                                    direction, 
00144                                    ord, link_padding, 
00145                                    place_goal.desired_retreat_distance, 0, actual_distance);
00146   if (actual_distance < place_goal.min_retreat_distance)
00147   {
00148     ROS_DEBUG_NAMED("manipulation","Object place: retreat incomplete (%f executed and %f desired)", 
00149              actual_distance, place_goal.min_retreat_distance);
00150     return Result(PlaceLocationResult::RETREAT_FAILED, false);
00151   }
00152 
00153   return Result(PlaceLocationResult::SUCCESS, true);
00154 }
00155 
00156 PlaceLocationResult 
00157 PlaceExecutor::prepareInterpolatedTrajectories(const object_manipulation_msgs::PlaceGoal &place_goal,
00158                                                const geometry_msgs::PoseStamped &place_location)
00159 {
00160   //compute gripper location for final place
00161   geometry_msgs::PoseStamped gripper_place_pose = computeGripperPose(place_location, 
00162                                                                      place_goal.grasp.grasp_pose, 
00163                                                                      handDescription().robotFrame(place_goal.arm_name));
00164   //publish marker
00165   if (marker_publisher_)
00166   {
00167     if (marker_id_ < 0)
00168     {
00169       marker_id_ = marker_publisher_->addGraspMarker(gripper_place_pose);
00170       marker_publisher_->colorGraspMarker(marker_id_, 1.0, 0.0, 1.0); //magenta
00171     }
00172     else
00173     {
00174       marker_publisher_->setMarkerPose(marker_id_, gripper_place_pose);
00175     }
00176   }
00177 
00178   //disable collisions between grasped object and table
00179   arm_navigation_msgs::OrderedCollisionOperations ord;
00180   arm_navigation_msgs::CollisionOperation coll;
00181   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00182   if (!place_goal.collision_object_name.empty() && !place_goal.collision_support_surface_name.empty())
00183   {
00184     coll.object1 = place_goal.collision_object_name;
00185     coll.object2 = place_goal.collision_support_surface_name;
00186     ord.collision_operations.push_back(coll);
00187   }
00188   if (place_goal.allow_gripper_support_collision)
00189   {
00190     coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00191     coll.object2 = place_goal.collision_support_surface_name;
00192     ord.collision_operations.push_back(coll);
00193   }
00194   ord.collision_operations = concat(ord.collision_operations, 
00195                                     place_goal.additional_collision_operations.collision_operations);
00196 
00197   //zero padding on fingertip links
00198   //std::vector<arm_navigation_msgs::LinkPadding> link_padding = 
00199   //  MechanismInterface::fingertipPadding(place_goal.arm_name, 0.0);
00200   std::vector<arm_navigation_msgs::LinkPadding> link_padding = 
00201     MechanismInterface::gripperPadding(place_goal.arm_name, 0.0);
00202 
00203   // padding on grasped object, which is still attached to the gripper
00204   arm_navigation_msgs::LinkPadding padding;
00205   padding.link_name = handDescription().attachedName(place_goal.arm_name);
00206   padding.padding = place_goal.place_padding;
00207   link_padding.push_back(padding);
00208   link_padding = concat(link_padding, place_goal.additional_link_padding);
00209 
00210   geometry_msgs::Vector3Stamped place_direction;
00211   place_direction.header.frame_id = place_goal.approach.direction.header.frame_id;
00212   place_direction.header.stamp = ros::Time::now();
00213   place_direction.vector = mechInterface().negate(place_goal.approach.direction.vector);
00214 
00215   //search backwards from place to pre-place
00216   std::vector<double> empty;
00217   float actual_distance;
00218   int error_code = mechInterface().getInterpolatedIK(place_goal.arm_name, 
00219                                                      gripper_place_pose, place_direction,
00220                                                      place_goal.approach.desired_distance,
00221                                                      empty, 
00222                                                      place_goal.grasp.grasp_posture, 
00223                                                      ord, link_padding, 
00224                                                      true, place_trajectory_, actual_distance);
00225   ROS_DEBUG_NAMED("manipulation"," Place trajectory: actual(%f), min(%f), desired (%f)", 
00226             actual_distance, place_goal.approach.min_distance, place_goal.approach.desired_distance);
00227 
00228   if (actual_distance < place_goal.approach.min_distance)
00229   {
00230     ROS_DEBUG_NAMED("manipulation","Place trajectory below min. threshold");
00231     if (place_trajectory_.points.empty())
00232     {
00233       ROS_DEBUG_NAMED("manipulation","Place trajectory empty; problem is with place location");
00234       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00235         return Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00236       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00237         return Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00238       else return Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);      
00239     }
00240     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00241       return Result(PlaceLocationResult::PREPLACE_IN_COLLISION, true);
00242     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00243       return Result(PlaceLocationResult::PREPLACE_OUT_OF_REACH, true);
00244     else return Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);      
00245   }
00246 
00247 
00248   //make sure first position is feasible with default padding
00249   if ( !mechInterface().checkStateValidity(place_goal.arm_name, place_trajectory_.points.front().positions,
00250                                            place_goal.additional_collision_operations,
00251                                            place_goal.additional_link_padding) )
00252   {
00253     ROS_DEBUG_NAMED("manipulation","First pose in place trajectory is unfeasible with default padding");
00254     return Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);
00255   }
00256   
00257 
00258   ord.collision_operations.clear();
00259   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00260   //disable all collisions on grasped object, since we are no longer holding it during the retreat
00261   if (!place_goal.collision_object_name.empty())
00262   {
00263     coll.object1 = place_goal.collision_object_name;
00264     coll.object2 = coll.COLLISION_SET_ALL;
00265     ord.collision_operations.push_back(coll);
00266   }
00267   if (place_goal.allow_gripper_support_collision)
00268   {
00269     coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00270     coll.object2 = place_goal.collision_support_surface_name;
00271     ord.collision_operations.push_back(coll);
00272   }
00273   ord.collision_operations = concat(ord.collision_operations, 
00274                                     place_goal.additional_collision_operations.collision_operations);
00275 
00276   geometry_msgs::Vector3Stamped retreat_direction;
00277   retreat_direction.header.stamp = ros::Time::now();
00278   retreat_direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00279   retreat_direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00280 
00281   //search from place to retreat, using solution from place as seed
00282   std::vector<double> place_joint_angles = place_trajectory_.points.back().positions;  
00283   mechInterface().getInterpolatedIK(place_goal.arm_name, 
00284                                     gripper_place_pose, retreat_direction,
00285                                     place_goal.desired_retreat_distance,
00286                                     place_joint_angles, 
00287                                     place_goal.grasp.pre_grasp_posture, 
00288                                     ord, link_padding, 
00289                                     false, retreat_trajectory_, actual_distance);
00290   ROS_DEBUG_NAMED("manipulation","Retreat trajectory: actual (%f), min (%f) and desired (%f)", actual_distance,
00291            place_goal.min_retreat_distance, place_goal.desired_retreat_distance);
00292 
00293   if (actual_distance < place_goal.min_retreat_distance)
00294   {
00295    ROS_DEBUG_NAMED("manipulation","Retreat trajectory below min. threshold");
00296     if (retreat_trajectory_.points.empty())
00297     {
00298       ROS_DEBUG_NAMED("manipulation","Retreat trajectory empty; problem is with place location");
00299       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00300         return Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00301       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00302         return Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00303       else return Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);      
00304     }
00305     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) 
00306       return Result(PlaceLocationResult::RETREAT_IN_COLLISION, true);
00307     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED)
00308       return Result(PlaceLocationResult::RETREAT_OUT_OF_REACH, true);
00309     else return Result(PlaceLocationResult::RETREAT_UNFEASIBLE, true);  
00310   }
00311 
00312   return Result(PlaceLocationResult::SUCCESS, true);
00313 }
00314 
00321 PlaceLocationResult 
00322 PlaceExecutor::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00323                              const geometry_msgs::PoseStamped&)
00324 {
00325   mechInterface().attemptTrajectory(place_goal.arm_name, place_trajectory_, true);
00326   return Result(PlaceLocationResult::SUCCESS, true);
00327 }
00328 
00329 
00339 PlaceLocationResult PlaceExecutor::place(const object_manipulation_msgs::PlaceGoal &place_goal,
00340                                          const geometry_msgs::PoseStamped &place_location)
00341 {
00342   //demo_synchronizer::getClient().rviz(1, "Collision models;IK contacts;Interpolated IK;Grasp execution");
00343 
00344   //compute interpolated trajectories
00345   PlaceLocationResult result = prepareInterpolatedTrajectories(place_goal, place_location);
00346   if (result.result_code != PlaceLocationResult::SUCCESS || place_goal.only_perform_feasibility_test) return result;
00347 
00348   //demo_synchronizer::getClient().sync(2, "Using motion planner to move arm to pre-place location");
00349   //demo_synchronizer::getClient().rviz(1, "Collision models;Planning goal;Environment contacts;Collision map");
00350 
00351   //whether we are using the constraints or not
00352   bool use_constraints = true;
00353 
00354   //check if we can actually understand the constraints
00355   if(!constraintsUnderstandable(place_goal.path_constraints))
00356   {
00357     ROS_WARN("Constraints passed to place executor are of types not yet handled. Ignoring them.");
00358     use_constraints = false;
00359   }
00360 
00361   if (place_goal.path_constraints.orientation_constraints.empty())
00362   {
00363     use_constraints = false;
00364   }
00365   
00366   if(use_constraints)
00367   {
00368     //recompute the pre-place pose from the already computed trajectory
00369     geometry_msgs::PoseStamped place_pose;
00370     place_pose.header.frame_id = place_location.header.frame_id;
00371     place_pose.header.stamp = ros::Time(0.0);
00372     //ROS_DEBUG_NAMED("manipulation","Place pose in frame: %s",place_pose.header.frame_id.c_str());
00373     if(!mechInterface().getFK(place_goal.arm_name, place_trajectory_.points.front().positions, place_pose))
00374     {
00375       ROS_ERROR("Could not re-compute pre-place pose based on trajectory");
00376       throw MechanismException("Could not re-compute pre-place pose based on trajectory");       
00377     }
00378     ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place with constraints");
00379     if (!mechInterface().moveArmConstrained(place_goal.arm_name, place_pose, 
00380                                             place_goal.additional_collision_operations,
00381                                             place_goal.additional_link_padding,
00382                                             place_goal.path_constraints, 
00383                                             place_trajectory_.points.front().positions[2], 
00384                                             false)) 
00385     {
00386       //TODO: in the future, this should be hard stop, with an informative message back 
00387       //to the caller saying the constraints have failed
00388       ROS_WARN("Object place: move_arm to pre-place with constraints failed. Trying again without constraints.");
00389       use_constraints = false;
00390     }
00391   }
00392 
00393   //try to go to the pre-place pose without constraints
00394   if(!use_constraints)
00395   {
00396     ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place without constraints");
00397     if ( !mechInterface().attemptMoveArmToGoal(place_goal.arm_name, place_trajectory_.points.front().positions,
00398                                                place_goal.additional_collision_operations,
00399                                                place_goal.additional_link_padding) ) 
00400     {
00401       ROS_DEBUG_NAMED("manipulation","Object place: move_arm (without constraints) to pre-place reports failure");
00402       return Result(PlaceLocationResult::MOVE_ARM_FAILED, true);
00403     }
00404   }
00405   ROS_DEBUG_NAMED("manipulation"," Arm moved to pre-place");
00406 
00407   //demo_synchronizer::getClient().sync(2, "Executing interpolated IK path for place, detaching and retreating");
00408   //demo_synchronizer::getClient().rviz(1, "Collision models");
00409 
00410   result = placeApproach(place_goal, place_location);
00411   if ( result.result_code != PlaceLocationResult::SUCCESS)
00412   {
00413     ROS_DEBUG_NAMED("manipulation"," Pre-place to place approach failed");
00414     return Result(PlaceLocationResult::PLACE_FAILED, false);
00415   }
00416   ROS_DEBUG_NAMED("manipulation"," Place trajectory done");
00417 
00418   mechInterface().detachAndAddBackObjectsAttachedToGripper(place_goal.arm_name, place_goal.collision_object_name);
00419   ROS_DEBUG_NAMED("manipulation"," Object detached");
00420 
00421   mechInterface().handPostureGraspAction(place_goal.arm_name, place_goal.grasp,
00422                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, -1);
00423   ROS_DEBUG_NAMED("manipulation"," Object released");
00424 
00425   result = retreat(place_goal);
00426   if (result.result_code != PlaceLocationResult::SUCCESS)
00427   {
00428     return Result(PlaceLocationResult::RETREAT_FAILED, false);
00429   }
00430   return Result(PlaceLocationResult::SUCCESS, true);
00431 }
00432 
00436 bool PlaceExecutor::constraintsUnderstandable(const arm_navigation_msgs::Constraints &constraints)
00437 {
00438   if(constraints.position_constraints.empty() && constraints.orientation_constraints.empty() && 
00439      constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00440     return true;
00441   if(constraints.orientation_constraints.size() == 1 && constraints.position_constraints.empty() 
00442      && constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00443     return true;
00444   return false;
00445 }
00446 
00447 PlaceLocationResult 
00448 ReactivePlaceExecutor::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00449                                      const geometry_msgs::PoseStamped &place_location)
00450 {
00451   //compute gripper location for final place
00452   geometry_msgs::PoseStamped gripper_place_pose = computeGripperPose(place_location, 
00453                                                                      place_goal.grasp.grasp_pose, 
00454                                                                      handDescription().robotFrame(place_goal.arm_name));
00455 
00456   //prepare the goal for reactive placing
00457   object_manipulation_msgs::ReactivePlaceGoal reactive_place_goal;  
00458   reactive_place_goal.arm_name = place_goal.arm_name;
00459   reactive_place_goal.collision_object_name = place_goal.collision_object_name;
00460   reactive_place_goal.collision_support_surface_name = place_goal.collision_support_surface_name;
00461   reactive_place_goal.trajectory = place_trajectory_;
00462   reactive_place_goal.final_place_pose = gripper_place_pose;
00463 
00464   //give the reactive place 1 minute to do its thing
00465   ros::Duration timeout = ros::Duration(60.0);
00466   ROS_DEBUG_NAMED("manipulation"," Calling the reactive place action");
00467   mechInterface().reactive_place_action_client_.client(place_goal.arm_name).sendGoal(reactive_place_goal);
00468   if ( !mechInterface().reactive_place_action_client_.client(place_goal.arm_name).waitForResult(timeout) )
00469   {
00470     ROS_ERROR("  Reactive place timed out");
00471     return Result(PlaceLocationResult::PLACE_FAILED, false);
00472   }
00473   object_manipulation_msgs::ReactivePlaceResult reactive_place_result = 
00474     *mechInterface().reactive_place_action_client_.client(place_goal.arm_name).getResult();
00475   if (reactive_place_result.manipulation_result.value != reactive_place_result.manipulation_result.SUCCESS)
00476   {
00477     ROS_ERROR("  Reactive place failed with error code %d", reactive_place_result.manipulation_result.value);
00478     return Result(PlaceLocationResult::PLACE_FAILED, false);
00479   }
00480   ROS_DEBUG_NAMED("manipulation","  Reactive place action succeeded");
00481   return Result(PlaceLocationResult::SUCCESS, true);
00482 }
00483 
00484 } //namespace object_grasping
place_executor.cpp
