descend_retreat_place.cpp
Go to the documentation of this 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 #include <object_manipulator/place_execution/descend_retreat_place.h>
00035 
00036 #include "object_manipulator/tools/mechanism_interface.h"
00037 #include "object_manipulator/tools/exceptions.h"
00038 #include "object_manipulator/tools/hand_description.h"
00039 #include "object_manipulator/tools/vector_tools.h"
00040 
00041 using arm_navigation_msgs::ArmNavigationErrorCodes;
00042 using object_manipulation_msgs::PlaceLocationResult;
00043 
00044 namespace object_manipulator {
00045 
00047 float StandardPlaceTester::EPS = 1.0e-6;
00048 
00049 // ---------------------------- Definitions ---------------------------------
00050 
00051 void PlaceTester::testPlaces(const object_manipulation_msgs::PlaceGoal &goal,
00052                              const std::vector<geometry_msgs::PoseStamped> &place_locations,
00053                              std::vector<PlaceExecutionInfo> &execution_info,
00054                              bool return_on_first_hit)
00055 {
00056   execution_info.clear();
00057   for (size_t i=0; i<place_locations.size(); i++)
00058   {
00059     ROS_DEBUG_NAMED("manipulation","Place tester: testing place %zd out of batch of %zd", i, place_locations.size());
00060     if (feedback_function_) feedback_function_(i);
00061     if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00062     PlaceExecutionInfo info;
00063     info.gripper_place_pose_ = place_locations[i];
00064     if (marker_publisher_)
00065     {
00066       info.marker_id_ = marker_publisher_->addGraspMarker(info.gripper_place_pose_);
00067       marker_publisher_->colorGraspMarker(info.marker_id_, 1.0, 0.0, 1.0); //magenta
00068     }
00069     testPlace(goal, place_locations[i], info);
00070     execution_info.push_back(info);
00071     if (info.result_.result_code == info.result_.SUCCESS && return_on_first_hit) return;
00072   }
00073 }
00074 
00075 void PlacePerformer::performPlaces(const object_manipulation_msgs::PlaceGoal &goal,
00076                                    const std::vector<geometry_msgs::PoseStamped> &place_locations,
00077                                    std::vector<PlaceExecutionInfo> &execution_info)
00078 {
00079   for (size_t i=0; i<place_locations.size(); i++)
00080   {
00081     if (feedback_function_) feedback_function_(i);
00082     if (interrupt_function_ && interrupt_function_()) throw InterruptRequestedException();
00083     if (i>= execution_info.size()) throw GraspException("Place Performer: not enough execution info provided");
00084     if (execution_info[i].result_.result_code != PlaceLocationResult::SUCCESS) continue;
00085     ROS_DEBUG_NAMED("manipulation","Place performer: trying place %zd out of batch of %zd", i, place_locations.size());
00086     performPlace(goal, place_locations[i], execution_info[i]);
00087     if (execution_info[i].result_.result_code == execution_info[i].result_.SUCCESS || 
00088         !execution_info[i].result_.continuation_possible) return;
00089   }
00090 }
00091 
00092 // ---------------------------- Testers ---------------------------------
00093 
00094 void StandardPlaceTester::testPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00095                                     const geometry_msgs::PoseStamped &place_location,
00096                                     PlaceExecutionInfo &execution_info)
00097 {
00098   //disable collisions between grasped object and table
00099   arm_navigation_msgs::OrderedCollisionOperations ord;
00100   arm_navigation_msgs::CollisionOperation coll;
00101   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00102   if (!place_goal.collision_object_name.empty() && !place_goal.collision_support_surface_name.empty())
00103   {
00104     coll.object1 = place_goal.collision_object_name;
00105     coll.object2 = place_goal.collision_support_surface_name;
00106     ord.collision_operations.push_back(coll);
00107   }
00108   if (place_goal.allow_gripper_support_collision)
00109   {
00110     coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00111     coll.object2 = place_goal.collision_support_surface_name;
00112     ord.collision_operations.push_back(coll);
00113   }
00114   ord.collision_operations = concat(ord.collision_operations, 
00115                                     place_goal.additional_collision_operations.collision_operations);
00116 
00117   //zero padding on fingertip links
00118   //std::vector<arm_navigation_msgs::LinkPadding> link_padding = 
00119   //  MechanismInterface::fingertipPadding(place_goal.arm_name, 0.0);
00120   std::vector<arm_navigation_msgs::LinkPadding> link_padding = 
00121     MechanismInterface::gripperPadding(place_goal.arm_name, 0.0);
00122 
00123   // padding on grasped object, which is still attached to the gripper
00124   arm_navigation_msgs::LinkPadding padding;
00125   padding.link_name = handDescription().attachedName(place_goal.arm_name);
00126   padding.padding = place_goal.place_padding;
00127   link_padding.push_back(padding);
00128   link_padding = concat(link_padding, place_goal.additional_link_padding);
00129 
00130   geometry_msgs::Vector3Stamped place_direction;
00131   place_direction.header.frame_id = place_goal.approach.direction.header.frame_id;
00132   place_direction.header.stamp = ros::Time::now();
00133   place_direction.vector = mechInterface().negate(place_goal.approach.direction.vector);
00134 
00135   //search backwards from place to pre-place
00136   std::vector<double> empty;
00137   float actual_distance;
00138   int error_code = mechInterface().getInterpolatedIK(place_goal.arm_name, 
00139                                                      execution_info.gripper_place_pose_, place_direction,
00140                                                      place_goal.approach.desired_distance,
00141                                                      empty, 
00142                                                      place_goal.grasp.grasp_posture, 
00143                                                      ord, link_padding, 
00144                                                      true, execution_info.descend_trajectory_, actual_distance);
00145   ROS_DEBUG_NAMED("manipulation"," Place trajectory: actual(%f), min(%f), desired (%f)", 
00146             actual_distance, place_goal.approach.min_distance, place_goal.approach.desired_distance);
00147 
00148   if (actual_distance < place_goal.approach.min_distance - EPS)
00149   {
00150     ROS_DEBUG_NAMED("manipulation","Place trajectory below min. threshold");
00151     if (execution_info.descend_trajectory_.points.empty())
00152     {
00153       ROS_DEBUG_NAMED("manipulation","Place trajectory empty; problem is with place location");
00154       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00155         execution_info.result_ = Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00156         return;
00157       }
00158       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00159         execution_info.result_ = Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00160         return;
00161       }
00162       else {
00163         execution_info.result_ = Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);      
00164         return;
00165       }
00166     }
00167     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00168       execution_info.result_ = Result(PlaceLocationResult::PREPLACE_IN_COLLISION, true);
00169       return;
00170     }
00171     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00172       execution_info.result_ = Result(PlaceLocationResult::PREPLACE_OUT_OF_REACH, true);
00173       return;
00174     }
00175     else {
00176       execution_info.result_ = Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);      
00177       return;
00178     }
00179   }
00180 
00181 
00182   //make sure first position is feasible with default padding
00183   if ( !mechInterface().checkStateValidity(place_goal.arm_name, 
00184                                            execution_info.descend_trajectory_.points.front().positions,
00185                                            place_goal.additional_collision_operations,
00186                                            place_goal.additional_link_padding) )
00187   {
00188     ROS_DEBUG_NAMED("manipulation","First pose in place trajectory is unfeasible with default padding");
00189     execution_info.result_ = Result(PlaceLocationResult::PREPLACE_UNFEASIBLE, true);
00190     return;
00191   }
00192   
00193 
00194   ord.collision_operations.clear();
00195   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00196   //disable all collisions on grasped object, since we are no longer holding it during the retreat
00197   if (!place_goal.collision_object_name.empty())
00198   {
00199     coll.object1 = place_goal.collision_object_name;
00200     coll.object2 = coll.COLLISION_SET_ALL;
00201     ord.collision_operations.push_back(coll);
00202   }
00203   if (place_goal.allow_gripper_support_collision)
00204   {
00205     coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00206     coll.object2 = place_goal.collision_support_surface_name;
00207     ord.collision_operations.push_back(coll);
00208   }
00209   ord.collision_operations = concat(ord.collision_operations, 
00210                                     place_goal.additional_collision_operations.collision_operations);
00211 
00212   geometry_msgs::Vector3Stamped retreat_direction;
00213   retreat_direction.header.stamp = ros::Time::now();
00214   retreat_direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00215   retreat_direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00216 
00217   //search from place to retreat, using solution from place as seed
00218   std::vector<double> place_joint_angles = execution_info.descend_trajectory_.points.back().positions;  
00219   mechInterface().getInterpolatedIK(place_goal.arm_name, 
00220                                     execution_info.gripper_place_pose_, retreat_direction,
00221                                     place_goal.desired_retreat_distance,
00222                                     place_joint_angles, 
00223                                     place_goal.grasp.pre_grasp_posture, 
00224                                     ord, link_padding, 
00225                                     false, execution_info.retreat_trajectory_, actual_distance);
00226   ROS_DEBUG_NAMED("manipulation","Retreat trajectory: actual (%f), min (%f) and desired (%f)", actual_distance,
00227            place_goal.min_retreat_distance, place_goal.desired_retreat_distance);
00228 
00229   if (actual_distance < place_goal.min_retreat_distance - EPS)
00230   {
00231    ROS_DEBUG_NAMED("manipulation","Retreat trajectory below min. threshold");
00232     if (execution_info.retreat_trajectory_.points.empty())
00233     {
00234       ROS_DEBUG_NAMED("manipulation","Retreat trajectory empty; problem is with place location");
00235       if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00236         execution_info.result_ = Result(PlaceLocationResult::PLACE_IN_COLLISION, true);
00237         return;
00238       }
00239       else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00240         execution_info.result_ = Result(PlaceLocationResult::PLACE_OUT_OF_REACH, true);
00241         return;
00242       }
00243       else {
00244         execution_info.result_ = Result(PlaceLocationResult::PLACE_UNFEASIBLE, true);      
00245         return;
00246       }
00247     }
00248     if (error_code == ArmNavigationErrorCodes::COLLISION_CONSTRAINTS_VIOLATED) {
00249       execution_info.result_ = Result(PlaceLocationResult::RETREAT_IN_COLLISION, true);
00250       return;
00251     }
00252     else if (error_code == ArmNavigationErrorCodes::JOINT_LIMITS_VIOLATED) {
00253       execution_info.result_ = Result(PlaceLocationResult::RETREAT_OUT_OF_REACH, true);
00254       return;
00255     }
00256     else {
00257       execution_info.result_ = Result(PlaceLocationResult::RETREAT_UNFEASIBLE, true);  
00258       return;
00259     }
00260   }
00261   execution_info.result_ = Result(PlaceLocationResult::SUCCESS, true);
00262 }
00263 
00264 // ---------------------------- Performers ---------------------------------
00265 
00280 PlaceLocationResult StandardPlacePerformer::retreat(const object_manipulation_msgs::PlaceGoal &place_goal)
00281 {
00282   arm_navigation_msgs::OrderedCollisionOperations ord;
00283   arm_navigation_msgs::CollisionOperation coll;
00284   coll.object1 = handDescription().gripperCollisionName(place_goal.arm_name);
00285   coll.operation = arm_navigation_msgs::CollisionOperation::DISABLE;
00286   //disable collision between gripper and object
00287   if (!place_goal.collision_object_name.empty())
00288   {
00289     coll.object2 = place_goal.collision_object_name;
00290     ord.collision_operations.push_back(coll);
00291   }
00292   //disable collision between gripper and table
00293   if (!place_goal.collision_support_surface_name.empty()) 
00294   {
00295     coll.object2 = place_goal.collision_support_surface_name;
00296     ord.collision_operations.push_back(coll);
00297   }
00298   ord.collision_operations = concat(ord.collision_operations, 
00299                                     place_goal.additional_collision_operations.collision_operations);
00300 
00301   //zero padding on gripper; shouldn't matter much due to collisions disabled above
00302   std::vector<arm_navigation_msgs::LinkPadding> link_padding 
00303     = concat(MechanismInterface::gripperPadding(place_goal.arm_name, 0.0),
00304              place_goal.additional_link_padding);
00305 
00306   geometry_msgs::Vector3Stamped direction;
00307   direction.header.stamp = ros::Time::now();
00308   direction.header.frame_id = handDescription().gripperFrame(place_goal.arm_name);
00309   direction.vector = mechInterface().negate( handDescription().approachDirection(place_goal.arm_name) );
00310 
00311   float actual_distance;
00312   mechInterface().translateGripper(place_goal.arm_name, 
00313                                    direction, 
00314                                    ord, link_padding, 
00315                                    place_goal.desired_retreat_distance, 0, actual_distance);
00316   if (actual_distance < place_goal.min_retreat_distance)
00317   {
00318     ROS_DEBUG_NAMED("manipulation","Object place: retreat incomplete (%f executed and %f desired)", 
00319              actual_distance, place_goal.min_retreat_distance);
00320     return Result(PlaceLocationResult::RETREAT_FAILED, false);
00321   }
00322 
00323   return Result(PlaceLocationResult::SUCCESS, true);
00324 }
00325 
00329 PlaceLocationResult 
00330 StandardPlacePerformer::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00331                                       const PlaceExecutionInfo &execution_info)
00332 {
00333   mechInterface().attemptTrajectory(place_goal.arm_name, execution_info.descend_trajectory_, true);
00334   return Result(PlaceLocationResult::SUCCESS, true);
00335 }
00336 
00337 
00338 void StandardPlacePerformer::performPlace(const object_manipulation_msgs::PlaceGoal &place_goal,
00339                                           const geometry_msgs::PoseStamped &place_location,
00340                                           PlaceExecutionInfo &execution_info)
00341 {
00342   //whether we are using the constraints or not
00343   bool use_constraints = true;
00344   //check if we can actually understand the constraints
00345   if(!constraintsUnderstandable(place_goal.path_constraints))
00346   {
00347     ROS_WARN("Constraints passed to place executor are of types not yet handled. Ignoring them.");
00348     use_constraints = false;
00349   }
00350   if (place_goal.path_constraints.orientation_constraints.empty())
00351   {
00352     use_constraints = false;
00353   }  
00354   if(use_constraints)
00355   {
00356     //recompute the pre-place pose from the already computed trajectory
00357     geometry_msgs::PoseStamped place_pose;
00358     place_pose.header.frame_id = place_location.header.frame_id;
00359     place_pose.header.stamp = ros::Time(0.0);
00360     //ROS_DEBUG_NAMED("manipulation","Place pose in frame: %s",place_pose.header.frame_id.c_str());
00361     if(!mechInterface().getFK(place_goal.arm_name, execution_info.descend_trajectory_.points.front().positions, 
00362                               place_pose))
00363     {
00364       ROS_ERROR("Could not re-compute pre-place pose based on trajectory");
00365       throw MechanismException("Could not re-compute pre-place pose based on trajectory");       
00366     }
00367     ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place with constraints");
00368     if (!mechInterface().moveArmConstrained(place_goal.arm_name, place_pose, 
00369                                             place_goal.additional_collision_operations,
00370                                             place_goal.additional_link_padding,
00371                                             place_goal.path_constraints, 
00372                                             execution_info.descend_trajectory_.points.front().positions[2], 
00373                                             false)) 
00374     {
00375       //TODO: in the future, this should be hard stop, with an informative message back 
00376       //to the caller saying the constraints have failed
00377       ROS_WARN("Object place: move_arm to pre-place with constraints failed. Trying again without constraints.");
00378       use_constraints = false;
00379     }
00380   }
00381   //try to go to the pre-place pose without constraints
00382   if(!use_constraints)
00383   {
00384     ROS_DEBUG_NAMED("manipulation","Attempting move arm to pre-place without constraints");
00385     if ( !mechInterface().attemptMoveArmToGoal(place_goal.arm_name, 
00386                                                execution_info.descend_trajectory_.points.front().positions,
00387                                                place_goal.additional_collision_operations,
00388                                                place_goal.additional_link_padding) ) 
00389     {
00390       ROS_DEBUG_NAMED("manipulation","Object place: move_arm (without constraints) to pre-place reports failure");
00391       execution_info.result_ = Result(PlaceLocationResult::MOVE_ARM_FAILED, true);
00392       return;
00393     }
00394   }
00395   ROS_DEBUG_NAMED("manipulation"," Arm moved to pre-place");
00396 
00397   execution_info.result_ = placeApproach(place_goal, execution_info);
00398   if ( execution_info.result_.result_code != PlaceLocationResult::SUCCESS)
00399   {
00400     ROS_DEBUG_NAMED("manipulation"," Pre-place to place approach failed");
00401     execution_info.result_ = Result(PlaceLocationResult::PLACE_FAILED, false);
00402     return;
00403   }
00404   ROS_DEBUG_NAMED("manipulation"," Place trajectory done");
00405 
00406   mechInterface().detachAndAddBackObjectsAttachedToGripper(place_goal.arm_name, place_goal.collision_object_name);
00407   ROS_DEBUG_NAMED("manipulation"," Object detached");
00408 
00409   mechInterface().handPostureGraspAction(place_goal.arm_name, place_goal.grasp,
00410                                          object_manipulation_msgs::GraspHandPostureExecutionGoal::RELEASE, -1);
00411   ROS_DEBUG_NAMED("manipulation"," Object released");
00412 
00413   execution_info.result_ = retreat(place_goal);
00414   if (execution_info.result_.result_code != PlaceLocationResult::SUCCESS)
00415   {
00416     execution_info.result_ = Result(PlaceLocationResult::RETREAT_FAILED, false);
00417     return;
00418   }
00419   execution_info.result_ = Result(PlaceLocationResult::SUCCESS, true);
00420 }
00421 
00425 bool StandardPlacePerformer::constraintsUnderstandable(const arm_navigation_msgs::Constraints &constraints)
00426 {
00427   if(constraints.position_constraints.empty() && constraints.orientation_constraints.empty() && 
00428      constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00429     return true;
00430   if(constraints.orientation_constraints.size() == 1 && constraints.position_constraints.empty() 
00431      && constraints.joint_constraints.empty() && constraints.visibility_constraints.empty())
00432     return true;
00433   return false;
00434 }
00435 
00436 PlaceLocationResult 
00437 ReactivePlacePerformer::placeApproach(const object_manipulation_msgs::PlaceGoal &place_goal,
00438                                       const PlaceExecutionInfo &execution_info)
00439 {
00440   //prepare the goal for reactive placing
00441   object_manipulation_msgs::ReactivePlaceGoal reactive_place_goal;  
00442   reactive_place_goal.arm_name = place_goal.arm_name;
00443   reactive_place_goal.collision_object_name = place_goal.collision_object_name;
00444   reactive_place_goal.collision_support_surface_name = place_goal.collision_support_surface_name;
00445   reactive_place_goal.trajectory = execution_info.descend_trajectory_;
00446   reactive_place_goal.final_place_pose = execution_info.gripper_place_pose_;
00447 
00448   //give the reactive place 1 minute to do its thing
00449   ros::Duration timeout = ros::Duration(60.0);
00450   ROS_DEBUG_NAMED("manipulation"," Calling the reactive place action");
00451   mechInterface().reactive_place_action_client_.client(place_goal.arm_name).sendGoal(reactive_place_goal);
00452   if ( !mechInterface().reactive_place_action_client_.client(place_goal.arm_name).waitForResult(timeout) )
00453   {
00454     ROS_ERROR("  Reactive place timed out");
00455     return Result(PlaceLocationResult::PLACE_FAILED, false);
00456   }
00457   object_manipulation_msgs::ReactivePlaceResult reactive_place_result = 
00458     *mechInterface().reactive_place_action_client_.client(place_goal.arm_name).getResult();
00459   if (reactive_place_result.manipulation_result.value != reactive_place_result.manipulation_result.SUCCESS)
00460   {
00461     ROS_ERROR("  Reactive place failed with error code %d", reactive_place_result.manipulation_result.value);
00462     return Result(PlaceLocationResult::PLACE_FAILED, false);
00463   }
00464   ROS_DEBUG_NAMED("manipulation","  Reactive place action succeeded");
00465   return Result(PlaceLocationResult::SUCCESS, true);
00466 }
00467 
00468 }


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Mon Oct 6 2014 02:59:50