mechanism_interface.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/tools/mechanism_interface.h"
00035 #include "object_manipulator/tools/hand_description.h"
00036 #include "object_manipulator/tools/exceptions.h"
00037 
00038 //#define PROF_ENABLED
00039 //#include <profiling/profiling.h>
00040 //PROF_DECLARE(INTERPOLATED_IK);
00041 //PROF_DECLARE(SET_PLANNING_SCENE);
00042 
00043 namespace object_manipulator {
00044 
00045 static const std::string IK_SERVICE_SUFFIX = "/constraint_aware_ik";
00046 static const std::string FK_SERVICE_SUFFIX = "/get_fk";
00047 static const std::string INTERPOLATED_IK_SERVICE_SUFFIX = "/interpolated_ik";
00048 static const std::string INTERPOLATED_IK_SET_PARAMS_SERVICE_SUFFIX = "/interpolated_ik_set_params";
00049 static const std::string IK_QUERY_SERVICE_SUFFIX = "/get_ik_solver_info";
00050 static const std::string GRASP_STATUS_SUFFIX = "/grasp_status";
00051 
00052 static const std::string SET_PLANNING_SCENE_DIFF_NAME = "environment_server/set_planning_scene_diff";
00053 static const std::string CHECK_STATE_VALIDITY_NAME = "planning_scene_validity_server/get_state_validity";
00054 static const std::string GET_ROBOT_STATE_NAME = "environment_server/get_robot_state";
00055 static const std::string NORMALIZE_SERVICE_NAME = "trajectory_filter_unnormalizer/filter_trajectory";
00056 static const std::string RESET_COLLISION_MAP_SERVICE_NAME = "collider_node/reset";
00057 
00058 static const std::string REACTIVE_GRASP_ACTION_SUFFIX = "/reactive_grasp";
00059 static const std::string REACTIVE_LIFT_ACTION_SUFFIX = "/reactive_lift";
00060 static const std::string REACTIVE_PLACE_ACTION_SUFFIX = "/reactive_place";
00061 static const std::string MOVE_ARM_ACTION_SUFFIX = "/move_arm";
00062 static const std::string TRAJECTORY_ACTION_SUFFIX = "/joint_trajectory";
00063 static const std::string HAND_POSTURE_ACTION_SUFFIX = "/hand_posture_execution";
00064 
00065 static const std::string SWITCH_CONTROLLER_SERVICE_NAME = "/switch_controller";
00066 static const std::string LIST_CONTROLLERS_SERVICE_NAME = "/list_controllers";
00067 static const std::string CARTESIAN_COMMAND_SUFFIX = "/cart/command_pose";
00068 static const std::string CARTESIAN_POSTURE_SUFFIX = "/cart/command_posture";
00069 //static const std::string CARTESIAN_GAINS_SUFFIX = "/cart/gains";
00070 
00071 static const std::string MOVE_ARM_PLANNER_ID = "SBLkConfig1";
00072 static const std::string MOVE_ARM_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path";
00073 static const std::string MOVE_ARM_CONSTRAINED_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path";
00074 
00075 static const std::string ATTACHED_COLLISION_TOPIC="attached_collision_object";
00076 
00077 static const std::string POINT_HEAD_ACTION_TOPIC = "/head_traj_controller/point_head_action";
00078 
00079 static const double OBJECT_POSITION_TOLERANCE_X = 0.02;
00080 static const double OBJECT_POSITION_TOLERANCE_Y = 0.02;
00081 static const double OBJECT_POSITION_TOLERANCE_Z = 0.02;
00082 
00083 MechanismInterface::MechanismInterface() : 
00084   root_nh_(""),priv_nh_("~"),
00085   cm_("robot_description"),
00086   planning_scene_state_(NULL),
00087   planning_scene_cache_empty_(true),
00088   cache_planning_scene_(false),
00089   //------------------- multi arm service clients -----------------------
00090   ik_query_client_("", IK_QUERY_SERVICE_SUFFIX, true),
00091   ik_service_client_("", IK_SERVICE_SUFFIX, true),
00092   fk_service_client_("",FK_SERVICE_SUFFIX,true),
00093   interpolated_ik_service_client_("", INTERPOLATED_IK_SERVICE_SUFFIX, true),
00094   interpolated_ik_set_params_client_("", INTERPOLATED_IK_SET_PARAMS_SERVICE_SUFFIX, true),
00095   grasp_status_client_("", GRASP_STATUS_SUFFIX, true),
00096   //------------------- simple service clients -----------------------
00097   check_state_validity_client_(CHECK_STATE_VALIDITY_NAME),
00098   joint_trajectory_normalizer_service_(NORMALIZE_SERVICE_NAME),
00099   switch_controller_service_(SWITCH_CONTROLLER_SERVICE_NAME),
00100   list_controllers_service_(LIST_CONTROLLERS_SERVICE_NAME),
00101   get_robot_state_client_(GET_ROBOT_STATE_NAME),
00102   set_planning_scene_diff_service_(SET_PLANNING_SCENE_DIFF_NAME),
00103   reset_collision_map_service_(RESET_COLLISION_MAP_SERVICE_NAME),
00104   //-------------------- multi arm action clients -----------------------
00105   reactive_grasp_action_client_("", REACTIVE_GRASP_ACTION_SUFFIX, true, true),
00106   reactive_lift_action_client_("", REACTIVE_LIFT_ACTION_SUFFIX, true, true),
00107   reactive_place_action_client_("", REACTIVE_PLACE_ACTION_SUFFIX, true, true),
00108   move_arm_action_client_("", MOVE_ARM_ACTION_SUFFIX, true, true),
00109   traj_action_client_("", TRAJECTORY_ACTION_SUFFIX, true, true),
00110   hand_posture_client_("", HAND_POSTURE_ACTION_SUFFIX, true, true),
00111   //-------------------- head action client -----------------------
00112   point_head_action_client_(POINT_HEAD_ACTION_TOPIC, true),
00113   //-------------------- multi arm topic publishers ---------------------
00114   cartesian_pub_("", CARTESIAN_COMMAND_SUFFIX, true),
00115   cartesian_posture_pub_("", CARTESIAN_POSTURE_SUFFIX, true)
00116   //cartesian_gains_pub_("", CARTESIAN_GAINS_SUFFIX, true)
00117 {
00118   //attached object publishing topic
00119   attached_object_pub_ = root_nh_.advertise<arm_navigation_msgs::AttachedCollisionObject>(ATTACHED_COLLISION_TOPIC, 10);
00120 
00121   //JointStates topic for current arm angles
00122   priv_nh_.param<std::string>("joint_states_topic", joint_states_topic_, "joint_states");
00123 
00124 }
00125 
00129 std::vector<std::string> MechanismInterface::getJointNames(std::string arm_name)
00130 {
00131   kinematics_msgs::GetKinematicSolverInfo::Request query_request;
00132   kinematics_msgs::GetKinematicSolverInfo::Response query_response;  
00133   if ( !ik_query_client_.client(arm_name).call(query_request, query_response) ) 
00134   {
00135     ROS_ERROR("Failed to call ik information query");
00136     throw MechanismException("Failed to call ik information query");
00137   }
00138   return query_response.kinematic_solver_info.joint_names;
00139 }
00140 
00141 void MechanismInterface::getRobotState(arm_navigation_msgs::RobotState& robot_state)
00142 {
00143   arm_navigation_msgs::GetRobotState::Request req;
00144   arm_navigation_msgs::GetRobotState::Response res;  
00145   if(!get_robot_state_client_.client().call(req,res)) 
00146   {
00147     ROS_ERROR("Mechanism interface: can't get current robot state");
00148     throw MechanismException("Mechanism interface: can't get current robot state");
00149   }
00150   robot_state = res.robot_state;
00151 }
00152 
00153 bool compareOrderedCollisionOperations(const arm_navigation_msgs::OrderedCollisionOperations& c1,
00154                                        const arm_navigation_msgs::OrderedCollisionOperations& c2)
00155 {
00156   if (c1.collision_operations.size() != c2.collision_operations.size()) return false;
00157   for (size_t i=0; i<c1.collision_operations.size(); i++)
00158   {
00159     if (c1.collision_operations[i].object1 != c2.collision_operations[i].object1 ) return false;
00160     if (c1.collision_operations[i].object2 != c2.collision_operations[i].object2 ) return false;
00161     if (c1.collision_operations[i].penetration_distance != 
00162         c2.collision_operations[i].penetration_distance ) return false;
00163     if (c1.collision_operations[i].operation != c2.collision_operations[i].operation ) return false;
00164   }
00165   return true;
00166 }
00167 
00168 bool compareLinkPadding(const std::vector<arm_navigation_msgs::LinkPadding> &l1,
00169                         const std::vector<arm_navigation_msgs::LinkPadding> &l2)
00170 {
00171   if (l1.size() != l2.size()) return false;
00172   for (size_t i=0; i<l1.size(); i++)
00173   {
00174     if (l1[i].link_name != l2[i].link_name) return false;
00175     if (l1[i].padding != l2[i].padding) return false;
00176   }
00177   return true;
00178 }
00179 
00180 bool MechanismInterface::cachePlanningScene(const arm_navigation_msgs::OrderedCollisionOperations& collision_operations,
00181                                             const std::vector<arm_navigation_msgs::LinkPadding> &link_padding)
00182 {
00183   static int cache_hits = 0;
00184   static int total_hits = 0;
00185   total_hits++;
00186   if (!cache_planning_scene_) 
00187   {
00188     ROS_DEBUG_NAMED("manipulation","Planning scene caching disabled");
00189     collision_operations_cache_ = collision_operations;
00190     link_padding_cache_ = link_padding;
00191     return false;
00192   }
00193   if (planning_scene_cache_empty_)
00194   {
00195     ROS_DEBUG_NAMED("manipulation","Planning scene cache empty.");
00196     collision_operations_cache_ = collision_operations;
00197     link_padding_cache_ = link_padding;
00198     planning_scene_cache_empty_ = false;
00199     return false;
00200   }
00201   if (!compareOrderedCollisionOperations(collision_operations, collision_operations_cache_))
00202   {
00203     ROS_DEBUG_NAMED("manipulation","Planning scene cache miss - collisions (hits: %d/%d).", cache_hits, total_hits);
00204     collision_operations_cache_ = collision_operations;
00205     link_padding_cache_ = link_padding;
00206     return false;
00207   }
00208   if (!compareLinkPadding(link_padding, link_padding_cache_))
00209   {
00210     ROS_DEBUG_NAMED("manipulation","Planning scene cache miss - padding (hits: %d/%d).", cache_hits, total_hits);
00211     collision_operations_cache_ = collision_operations;
00212     link_padding_cache_ = link_padding;
00213     return false;
00214   }
00215   cache_hits++;
00216   ROS_DEBUG_NAMED("manipulation", "Planning scene cache hit (hits: %d/%d).", cache_hits, total_hits);
00217   return true;
00218 }
00219   
00220 void MechanismInterface::getPlanningScene(const arm_navigation_msgs::OrderedCollisionOperations& collision_operations,
00221                                           const std::vector<arm_navigation_msgs::LinkPadding> &link_padding)
00222 {
00223   //if (cachePlanningScene(collision_operations, link_padding)) return;
00224   arm_navigation_msgs::SetPlanningSceneDiff::Request planning_scene_req;
00225   planning_scene_req.planning_scene_diff.link_padding = link_padding;
00226   planning_scene_req.operations = collision_operations;
00227   arm_navigation_msgs::SetPlanningSceneDiff::Response planning_scene_res;  
00228   //PROF_COUNT(SET_PLANNING_SCENE);
00229   //PROF_START_TIMER(SET_PLANNING_SCENE);
00230   //ROS_INFO("mechanism_interface: setting the planning scene diff");
00231   if(!set_planning_scene_diff_service_.client().call(planning_scene_req, planning_scene_res)) 
00232   {
00233     ROS_ERROR("Failed to set planning scene diff");
00234     throw MechanismException("Failed to set planning scene diff");
00235   }
00236   
00237   if(planning_scene_state_ != NULL) {
00238     cm_.revertPlanningScene(planning_scene_state_);
00239   }
00240   planning_scene_state_ = cm_.setPlanningScene(planning_scene_res.planning_scene);
00241   //PROF_STOP_TIMER(SET_PLANNING_SCENE);
00242 }
00243 
00244 trajectory_msgs::JointTrajectory MechanismInterface::assembleJointTrajectory(std::string arm_name, 
00245                                            const std::vector< std::vector<double> > &positions, 
00246                                            float time_per_segment)
00247 {
00248   trajectory_msgs::JointTrajectory trajectory;
00249   trajectory.header.frame_id = handDescription().robotFrame(arm_name);
00250   trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00251   trajectory.joint_names = getJointNames(arm_name);
00252   float current_time = 0;
00253   for (size_t i=0; i<positions.size(); i++)
00254   {
00255     current_time += time_per_segment;
00256     if (positions[i].size() != trajectory.joint_names.size())
00257     {
00258       ROS_ERROR("Mechanism interface: requested trajectory does not have enough joint values");
00259       throw MechanismException("Mechanism interface: requested trajectory does not have enough joint values");
00260     }
00261     trajectory_msgs::JointTrajectoryPoint point;
00262     point.positions = positions[i];
00263     point.time_from_start = ros::Duration(current_time);
00264     trajectory.points.push_back(point);
00265   }
00266   return trajectory;
00267 }
00268 
00269 void MechanismInterface::attemptTrajectory(std::string arm_name,
00270                                            const std::vector< std::vector<double> > &positions,
00271                                            bool unnormalize,
00272                                            float time_per_segment)
00273 {
00274   attemptTrajectory(arm_name, assembleJointTrajectory(arm_name, positions, time_per_segment), unnormalize);
00275 }
00276 
00277 
00278 void MechanismInterface::unnormalizeTrajectory(std::string arm_name, 
00279                                            const trajectory_msgs::JointTrajectory &input_trajectory,
00280                                            trajectory_msgs::JointTrajectory &normalized_trajectory)
00281 {    
00282   arm_navigation_msgs::FilterJointTrajectory service_call;
00283   getRobotState(service_call.request.start_state);
00284   service_call.request.trajectory = input_trajectory;
00285   service_call.request.allowed_time = ros::Duration(2.0);
00286   if ( !joint_trajectory_normalizer_service_.client().call(service_call) )
00287   {
00288     ROS_ERROR("Mechanism interface: joint trajectory normalizer service call failed");
00289     throw MechanismException("joint trajectory normalizer service call failed");
00290   } 
00291   normalized_trajectory = service_call.response.trajectory;
00292 }
00293 
00294 
00295 void MechanismInterface::attemptTrajectory(std::string arm_name, 
00296                                            const trajectory_msgs::JointTrajectory &trajectory, 
00297                                            bool unnormalize)
00298 {
00299   if (trajectory.points.empty()) 
00300   {
00301     ROS_ERROR("attemptTrajectory called with empty trajectory");
00302     throw MechanismException("attemptTrajectory called with empty trajectory");
00303   }
00304 
00305   //make sure joint controllers are running
00306   if(!checkController(jointControllerName(arm_name)))
00307      switchToJoint(arm_name);
00308 
00309   pr2_controllers_msgs::JointTrajectoryGoal goal;
00310   if (unnormalize)
00311   {
00312     unnormalizeTrajectory(arm_name, trajectory, goal.trajectory);
00313   }
00314   else
00315   {
00316     goal.trajectory = trajectory;
00317   }
00318   //force it to wait for client here, so that the duration below is not wasted
00319   traj_action_client_.client(arm_name);
00320   goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00321 
00322   //wait 5 seconds more that the whole trajectory is supposed to take
00323   ros::Duration timeout = ros::Duration(1.0) + trajectory.points.back().time_from_start + ros::Duration(5.0);
00324   traj_action_client_.client(arm_name).sendGoal(goal);
00325   if ( !traj_action_client_.client(arm_name).waitForResult(timeout) ) 
00326   {
00327     ROS_ERROR("  Trajectory timed out");
00328     throw MechanismException("trajectory timed out");
00329   }
00330 }
00331 
00332 void MechanismInterface::setInterpolatedIKParams(std::string arm_name, int num_steps,
00333                                                  int collision_check_resolution, bool start_from_end)
00334 {
00335   interpolated_ik_motion_planner::SetInterpolatedIKMotionPlanParams srv;
00336   srv.request.num_steps = num_steps;
00337   srv.request.consistent_angle = M_PI/6;
00338   srv.request.collision_check_resolution = collision_check_resolution;
00339   srv.request.steps_before_abort = 0;
00340   srv.request.pos_spacing = 0.01; //ignored if num_steps !=0
00341   srv.request.rot_spacing = 0.1;  //ignored if num_steps !=0
00342   srv.request.collision_aware = true;
00343   srv.request.start_from_end = start_from_end;
00344   if (!interpolated_ik_set_params_client_.client(arm_name).call(srv))
00345   {
00346     ROS_ERROR("Failed to set Interpolated IK server parameters");
00347     throw MechanismException("Failed to set Interpolated IK server parameters");
00348   }
00349 }
00350 
00351 bool MechanismInterface::moveArmToPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00352                                        const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00353                                        const std::vector<arm_navigation_msgs::LinkPadding> &link_padding)
00354 {
00355   kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
00356   if(!getIKForPose(arm_name, desired_pose,ik_response, collision_operations, link_padding)) return false;
00357   if(!attemptMoveArmToGoal(arm_name, ik_response.solution.joint_state.position, collision_operations, link_padding))
00358     return false;
00359   return true;
00360 }
00361 
00362 bool MechanismInterface::getFK(std::string arm_name, 
00363                                std::vector<double> positions, 
00364                                geometry_msgs::PoseStamped &pose_stamped)
00365 {
00366  // define the service messages
00367  kinematics_msgs::GetPositionFK::Request  fk_request;
00368  kinematics_msgs::GetPositionFK::Response fk_response;
00369  fk_request.header.frame_id = pose_stamped.header.frame_id;
00370  fk_request.header.stamp = pose_stamped.header.stamp;
00371  fk_request.fk_link_names.resize(1);
00372  fk_request.fk_link_names[0] = handDescription().gripperFrame(arm_name);
00373  fk_request.robot_state.joint_state.position = positions;
00374  fk_request.robot_state.joint_state.name = getJointNames(arm_name);
00375  if( !fk_service_client_.client(arm_name).call(fk_request,fk_response) ) 
00376    {
00377      ROS_ERROR("FK Service Call failed altogether");
00378      throw MechanismException("FK Service Call failed altogether");
00379    }
00380  if (fk_response.error_code.val != fk_response.error_code.SUCCESS)
00381    {
00382     ROS_ERROR("Get FK failed with error code %d", fk_response.error_code.val);
00383     return false;
00384    }
00385  pose_stamped = fk_response.pose_stamped[0];
00386  return true;
00387 }
00388 
00389 bool MechanismInterface::getIKForPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00390                                       kinematics_msgs::GetConstraintAwarePositionIK::Response& ik_response,
00391                                       const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00392                                       const std::vector<arm_navigation_msgs::LinkPadding> &link_padding)
00393 {
00394   //prepare the planning scene
00395   getPlanningScene(collision_operations, link_padding);
00396   //call collision-aware ik
00397   kinematics_msgs::GetConstraintAwarePositionIK::Request ik_request;
00398   ik_request.ik_request.ik_link_name = handDescription().gripperFrame(arm_name);
00399   ik_request.ik_request.pose_stamped.pose = desired_pose.pose;
00400   ik_request.ik_request.pose_stamped.header.stamp = desired_pose.header.stamp;
00401   ik_request.ik_request.pose_stamped.header.frame_id = desired_pose.header.frame_id;
00402   ik_request.ik_request.ik_seed_state.joint_state.name = getJointNames(arm_name);
00403   ik_request.ik_request.ik_seed_state.joint_state.position.resize(7, 0.0);
00404   ik_request.timeout = ros::Duration(2.0);
00405   if( !ik_service_client_.client(arm_name).call(ik_request,ik_response) ) 
00406   {
00407     ROS_ERROR("IK Service Call failed altogether");
00408     throw MechanismException("IK Service Call failed altogether");
00409   }
00410   if (ik_response.error_code.val != ik_response.error_code.SUCCESS)
00411   {
00412     ROS_ERROR("Get IK failed with error code %d", ik_response.error_code.val);
00413     return false;
00414   }
00415   return true;
00416 }
00417 
00418 bool MechanismInterface::checkStateValidity(std::string arm_name, const std::vector<double> &joint_values,
00419                                           const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00420                                             const std::vector<arm_navigation_msgs::LinkPadding> &link_padding)
00421 {
00422   //prepare the planning scene
00423   getPlanningScene(collision_operations, link_padding);
00424   //call check state validity
00425   arm_navigation_msgs::GetStateValidity::Request req;
00426   arm_navigation_msgs::GetStateValidity::Response res;
00427   req.group_name = handDescription().armGroup(arm_name);
00428   if (!joint_values.empty())
00429   {
00430     req.robot_state.joint_state.name = getJointNames(arm_name);
00431     req.robot_state.joint_state.position = joint_values;
00432     if ( req.robot_state.joint_state.name.size() != joint_values.size() )
00433     {
00434       throw MechanismException("Wrong number of joint values for checkStateValidity");
00435     }
00436     req.robot_state.joint_state.header.stamp = ros::Time::now();
00437   }
00438   req.check_collisions = true;
00439   if(!check_state_validity_client_.client().call(req,res))
00440   {
00441     throw MechanismException("Call to check state validity client failed");
00442   }
00443 
00444   if (res.error_code.val == res.error_code.SUCCESS) return true;
00445   return false;
00446 }
00447 
00449 double getJointPosition( std::string name, const arm_navigation_msgs::RobotState& robot_state)
00450 {
00451   for (size_t i=0; i<robot_state.joint_state.name.size(); i++)
00452   {
00453     if (robot_state.joint_state.name[i] == name)
00454     {
00455       ROS_ASSERT(robot_state.joint_state.position.size() > i);
00456       return robot_state.joint_state.position[i];
00457     }
00458   }
00459   ROS_ERROR_STREAM("Joint " << name << " not found in robot state");
00460   return 0.0;
00461 }
00462 
00475 int MechanismInterface::getInterpolatedIK(std::string arm_name,
00476                                           geometry_msgs::PoseStamped start_pose,
00477                                           geometry_msgs::Vector3Stamped direction,
00478                                           float desired_trajectory_length,
00479                                           const std::vector<double> &seed_joint_position,
00480                                           const sensor_msgs::JointState &joint_state,
00481                                           const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00482                                           const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00483                                           bool reverse_trajectory,
00484                                           trajectory_msgs::JointTrajectory &trajectory,
00485                                           float &actual_trajectory_length)
00486 {
00487   //first compute the desired end pose
00488   //make sure the input is normalized
00489   geometry_msgs::Vector3Stamped direction_norm = direction;
00490   direction_norm.vector = normalize(direction.vector);
00491   //multiply by the length
00492   desired_trajectory_length = fabs(desired_trajectory_length);
00493   direction_norm.vector.x *= desired_trajectory_length;
00494   direction_norm.vector.y *= desired_trajectory_length;
00495   direction_norm.vector.z *= desired_trajectory_length;
00496   geometry_msgs::PoseStamped end_pose = translateGripperPose(direction_norm, start_pose, arm_name);
00497  
00498   //hard-coded for now
00499   float max_step_size = 0.01;
00500   unsigned int collision_check_resolution = 2;
00501  
00502   //compute the number of steps  
00503   unsigned int num_steps = (unsigned int)ceil(desired_trajectory_length / fabs(max_step_size));
00504   float actual_step_size = desired_trajectory_length / num_steps;
00505 
00506   ROS_DEBUG_NAMED("manipulation","Trajectory details: length %f, requested num steps: %d, actual step size: %f",
00507            desired_trajectory_length, num_steps, actual_step_size);
00508 
00509   if (reverse_trajectory)
00510   {
00511     std::swap(start_pose, end_pose);
00512   }
00513 
00514   //recall that here we setting the number of points in trajectory, which is steps+1
00515   setInterpolatedIKParams(arm_name, num_steps+1, collision_check_resolution, reverse_trajectory);
00516 
00517   arm_navigation_msgs::RobotState start_state;
00518   start_state.multi_dof_joint_state.child_frame_ids.push_back(handDescription().gripperFrame(arm_name));
00519   start_state.multi_dof_joint_state.poses.push_back(start_pose.pose);
00520   start_state.multi_dof_joint_state.frame_ids.push_back(start_pose.header.frame_id);
00521   start_state.multi_dof_joint_state.stamp = ros::Time::now();
00522 
00523   //pass the seeds for the IK
00524   if (!seed_joint_position.empty())
00525   { 
00526     //the caller has provided seeds for planned joints
00527     //we are silently assuming that the values passed in match our joint names for IK
00528     start_state.joint_state.name = getJointNames(arm_name);
00529     if (seed_joint_position.size() != start_state.joint_state.name.size())
00530     {
00531       ROS_ERROR("Interpolated IK request: seed_joint_position does not match joint names");
00532       throw MechanismException("Interpolated IK request: seed_joint_position does not match joint names");
00533     }
00534     start_state.joint_state.position = seed_joint_position;
00535   }
00536   else
00537   {
00538     //no seeds have been passed in, use current robot state
00539     //grab the latest JointState message and read in joint positions
00540     arm_navigation_msgs::RobotState robot_state;
00541     getRobotState(robot_state);
00542     start_state.joint_state.name = getJointNames(arm_name);
00543     for (size_t i=0; i<start_state.joint_state.name.size(); i++)
00544     {
00545       start_state.joint_state.position.push_back( getJointPosition(start_state.joint_state.name[i], robot_state) );
00546     }
00547   }
00548 
00549   //pass the desired values of non-planned joints, if any
00550   for (size_t i=0; i<joint_state.name.size(); i++)
00551   {
00552     start_state.joint_state.name.push_back(joint_state.name[i]);
00553     start_state.joint_state.position.push_back(joint_state.position[i]);
00554   }
00555   
00556   arm_navigation_msgs::PositionConstraint position_constraint;
00557   arm_navigation_msgs::OrientationConstraint orientation_constraint;
00558   arm_navigation_msgs::poseStampedToPositionOrientationConstraints(end_pose, handDescription().gripperFrame(arm_name),
00559                                                                     position_constraint, 
00560                                                                     orientation_constraint);
00561   arm_navigation_msgs::Constraints goal_constraints;
00562   goal_constraints.position_constraints.push_back(position_constraint);
00563   goal_constraints.orientation_constraints.push_back(orientation_constraint);
00564 
00565   arm_navigation_msgs::GetMotionPlan motion_plan;
00566   motion_plan.request.motion_plan_request.start_state = start_state;
00567   motion_plan.request.motion_plan_request.goal_constraints = goal_constraints;
00568 
00569   //prepare the planning scene
00570   getPlanningScene(collision_operations, link_padding);
00571 
00572   //PROF_COUNT(INTERPOLATED_IK);
00573   //PROF_START_TIMER(INTERPOLATED_IK);
00574   if ( !interpolated_ik_service_client_.client(arm_name).call(motion_plan) ) 
00575   {
00576     ROS_ERROR("  Call to Interpolated IK service failed");
00577     throw MechanismException("Call to Interpolated IK service failed");
00578   }
00579   //PROF_STOP_TIMER(INTERPOLATED_IK);
00580 
00581   trajectory.points.clear();
00582   trajectory.joint_names = motion_plan.response.trajectory.joint_trajectory.joint_names;
00583 
00584   if (motion_plan.response.trajectory_error_codes.empty()) 
00585   {
00586     ROS_ERROR("  Interpolated IK: empty trajectory received");
00587     throw MechanismException("Interpolated IK: empty trajectory received");
00588   }
00589   
00590   int error_code = arm_navigation_msgs::ArmNavigationErrorCodes::SUCCESS;
00591   if (!reverse_trajectory)
00592   {
00593     for (size_t i=0; i<motion_plan.response.trajectory_error_codes.size(); i++) 
00594     {
00595       if ( motion_plan.response.trajectory_error_codes[i].val == motion_plan.response.trajectory_error_codes[i].SUCCESS )
00596       {
00597         trajectory.points.push_back( motion_plan.response.trajectory.joint_trajectory.points[i] );
00598       } 
00599       else 
00600       {
00601         ROS_DEBUG_NAMED("manipulation","  Interpolated IK failed on step %d (forward towards %d) with error code %d", 
00602                  (int) i, 
00603                  (int) motion_plan.response.trajectory_error_codes.size() - 1, 
00604                  motion_plan.response.trajectory_error_codes[i].val);
00605         error_code = motion_plan.response.trajectory_error_codes[i].val;
00606         break;
00607       }
00608     }
00609   }
00610   else
00611   {
00612     size_t first_success = 0;
00613     while ( first_success < motion_plan.response.trajectory_error_codes.size() &&
00614             motion_plan.response.trajectory_error_codes[first_success].val != 
00615             motion_plan.response.trajectory_error_codes[first_success].SUCCESS ) first_success ++;
00616     if (first_success != 0)
00617     {
00618       ROS_DEBUG_NAMED("manipulation","  Interpolation failed on step %d (backwards from %d) with error code %d",
00619                (int) first_success - 1,
00620                (int) motion_plan.response.trajectory_error_codes.size() - 1,
00621                motion_plan.response.trajectory_error_codes[first_success - 1].val);
00622       error_code = motion_plan.response.trajectory_error_codes[first_success - 1].val;
00623     }
00624     else
00625     {
00626       ROS_DEBUG_NAMED("manipulation","  Interpolation trajectory complete (backwards from %d points)",
00627                (int) motion_plan.response.trajectory_error_codes.size());
00628       
00629     }
00630     for (size_t i=first_success; i < motion_plan.response.trajectory_error_codes.size(); i++) 
00631     {
00632       if ( motion_plan.response.trajectory_error_codes[i].val == motion_plan.response.trajectory_error_codes[i].SUCCESS )
00633       {
00634         trajectory.points.push_back( motion_plan.response.trajectory.joint_trajectory.points[i] );
00635       } 
00636       else 
00637       {
00638         ROS_ERROR("  Interpolated IK: unexpected behavior for error codes: step %d has error code %d",
00639                   (int) i, motion_plan.response.trajectory_error_codes[i].val);
00640         throw MechanismException("Interpolated IK: unexpected behavior for error codes");
00641       }
00642     }
00643   }
00644 
00645   if (!trajectory.points.empty()) actual_trajectory_length = actual_step_size * (trajectory.points.size()-1);
00646   else actual_trajectory_length = 0.0;
00647   return error_code;
00648 }
00649 
00650 void MechanismInterface::cancelMoveArmGoal(std::string arm_name)
00651 {
00652   move_arm_action_client_.client(arm_name).cancelGoal();
00653 }
00654 
00655 actionlib::SimpleClientGoalState MechanismInterface::getMoveArmState(std::string arm_name)
00656 {
00657   actionlib::SimpleClientGoalState state = move_arm_action_client_.client(arm_name).getState();
00658   return state;
00659 }
00660 
00661 bool MechanismInterface::clearMoveArmGoals(double timeout)
00662 {
00663   //if move_arm is running, cancel its goal
00664   actionlib::SimpleClientGoalState state = getMoveArmState("right_arm");
00665   if(state == actionlib::SimpleClientGoalState::ACTIVE || state == actionlib::SimpleClientGoalState::PENDING)
00666   {
00667     ROS_INFO("move_arm for the right arm is running!  Attempting to cancel goals");
00668     ros::Time start_time = ros::Time::now();
00669     cancelMoveArmGoal("right_arm");
00670     while(state == actionlib::SimpleClientGoalState::ACTIVE || state == actionlib::SimpleClientGoalState::PENDING)
00671     {
00672       ros::Duration(0.1).sleep();
00673       state = getMoveArmState("right_arm");
00674       if(ros::Time::now() - start_time > ros::Duration(timeout))
00675       {
00676         ROS_WARN("Timed out while waiting for move arm goal to finish");
00677         return false;
00678       } 
00679     }
00680   }
00681   state = getMoveArmState("left_arm");
00682   if(state == actionlib::SimpleClientGoalState::ACTIVE || state == actionlib::SimpleClientGoalState::PENDING)
00683   {
00684     ROS_INFO("move_arm for the left arm is running!  Attempting to cancel goals");
00685     ros::Time start_time = ros::Time::now();
00686     cancelMoveArmGoal("left_arm");
00687     while(state == actionlib::SimpleClientGoalState::ACTIVE || state == actionlib::SimpleClientGoalState::PENDING)
00688     {
00689       ros::Duration(0.1).sleep();
00690       state = getMoveArmState("left_arm");
00691       if(ros::Time::now() - start_time > ros::Duration(timeout))
00692       {
00693         ROS_WARN("Timed out while waiting for move arm goal to finish");
00694         return false;
00695       } 
00696     }
00697   }
00698   ROS_INFO("Done clearing move arm goals");
00699   return true;
00700 }
00701 
00702 bool MechanismInterface::attemptMoveArmToGoal(std::string arm_name, const std::vector<double> &desired_joint_values,
00703                                               const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00704                                               const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00705                                               int max_tries, bool reset_map_if_stuck, double timeout) 
00706 {
00707   //make sure joint controllers are running
00708   if(!checkController(jointControllerName(arm_name)))
00709      switchToJoint(arm_name);
00710 
00711   int num_tries = 0;  
00712   arm_navigation_msgs::MoveArmGoal move_arm_goal;
00713   move_arm_goal.planning_scene_diff.link_padding = link_padding;
00714   move_arm_goal.operations = collision_operations;
00715 
00716   move_arm_goal.motion_plan_request.group_name = handDescription().armGroup(arm_name);
00717   move_arm_goal.motion_plan_request.num_planning_attempts = 1;
00718   move_arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00719   move_arm_goal.motion_plan_request.planner_id = MOVE_ARM_PLANNER_ID;
00720   move_arm_goal.planner_service_name = MOVE_ARM_PLANNER_SERVICE_NAME;
00721   
00722 
00723   move_arm_goal.operations = collision_operations;
00724   move_arm_goal.planning_scene_diff.link_padding = link_padding;
00725 
00726   std::vector<std::string> joint_names = getJointNames(arm_name);
00727   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints.resize(joint_names.size());
00728   for(unsigned int i = 0; i < desired_joint_values.size(); i++) 
00729   {
00730     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names.at(i);
00731     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].position = desired_joint_values[i];
00732     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = .08;
00733     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = .08;
00734   }
00735   
00736   bool success = false;
00737   arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00738   while(num_tries < max_tries)
00739   {
00740     move_arm_action_client_.client(arm_name).sendGoal(move_arm_goal);
00741     bool withinWait = move_arm_action_client_.client(arm_name).waitForResult(ros::Duration(timeout));
00742     if(!withinWait) 
00743     {
00744       move_arm_action_client_.client(arm_name).cancelGoal();
00745       ROS_DEBUG_NAMED("manipulation","   Move arm goal could not be achieved by move_arm in the allowed duration");
00746       success = false;
00747       num_tries++;
00748       arm_navigation_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00749       error_code.val = error_code.TIMED_OUT;
00750       modifyMoveArmGoal(move_arm_goal,error_code,move_arm_result.contacts);
00751       continue;
00752     }
00753     actionlib::SimpleClientGoalState state = move_arm_action_client_.client(arm_name).getState();
00754     if(state == actionlib::SimpleClientGoalState::SUCCEEDED) 
00755     {
00756       ROS_DEBUG_NAMED("manipulation","  Move arm: position was successfully achieved");
00757       success = true;
00758       break;
00759     } 
00760     else 
00761     {
00762       arm_navigation_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00763       ROS_DEBUG_NAMED("manipulation","   Move arm: non-success state was reached. Reason: %s",
00764                (arm_navigation_msgs::armNavigationErrorCodeToString(move_arm_result.error_code)).c_str());
00765       ROS_DEBUG_NAMED("manipulation","   num_tries: %d, max_tries: %d",num_tries,max_tries);
00766       success = false;
00767       num_tries++;
00768       error_code = move_arm_result.error_code;
00769 
00770       //reset the collision map and wait to repopulate it before trying again
00771       if(reset_map_if_stuck && error_code.val == error_code.START_STATE_IN_COLLISION && num_tries < max_tries)
00772       {
00773         std_srvs::Empty srv;
00774         if ( !reset_collision_map_service_.client().call(srv) )
00775         {
00776           ROS_ERROR("Mechanism interface: reset collision map service call failed");
00777         }
00778         //tried to reset and repopulate twice already; just reset and try again right away
00779         if(num_tries <= 2){
00780           ros::Duration(5.0).sleep();
00781           ROS_INFO("move arm is stuck!  Resetting the collision map and repopulating!");
00782         }
00783         else{
00784           ROS_ERROR("move arm is still stuck!  Resetting the collision map and NOT repopulating");
00785         }
00786       }
00787       //modifyMoveArmGoal(move_arm_goal,error_code,move_arm_result.contacts);
00788       continue;
00789     }
00790   }
00792   if (!success && error_code.val == error_code.START_STATE_IN_COLLISION)
00793   {
00794     ROS_ERROR("move arm is still stuck!  Throwing MoveArmStuckException!");
00795     throw MoveArmStuckException();
00796   }
00797   return success;
00798 }
00799 
00800 
00801 void MechanismInterface::modifyMoveArmGoal(arm_navigation_msgs::MoveArmGoal &move_arm_goal, 
00802                                            arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
00803                                            std::vector<arm_navigation_msgs::ContactInformation> &contact_info_)
00804 {
00805   double allowed_penetration_depth = 0.03;
00806   if(error_code.val == error_code.START_STATE_IN_COLLISION)
00807   {
00808     std::vector<arm_navigation_msgs::AllowedContactSpecification> allowed_contacts;
00809     for(unsigned int i=0; i < contact_info_.size(); i++)
00810     {
00811       if(contact_info_[i].depth < allowed_penetration_depth)
00812       {
00813         arm_navigation_msgs::AllowedContactSpecification allowed_contact_tmp;
00814         allowed_contact_tmp.shape.type = allowed_contact_tmp.shape.BOX;
00815         allowed_contact_tmp.shape.dimensions.resize(3);
00816         allowed_contact_tmp.shape.dimensions[0] = 0.03;
00817         allowed_contact_tmp.shape.dimensions[1] = 0.03;
00818         allowed_contact_tmp.shape.dimensions[2] = 0.03;
00819         allowed_contact_tmp.pose_stamped.pose.position = contact_info_[i].position;
00820         allowed_contact_tmp.pose_stamped.pose.orientation.x = 0.0;
00821         allowed_contact_tmp.pose_stamped.pose.orientation.y = 0.0;
00822         allowed_contact_tmp.pose_stamped.pose.orientation.z = 0.0;
00823         allowed_contact_tmp.pose_stamped.pose.orientation.w = 1.0;
00824         allowed_contact_tmp.pose_stamped.header.stamp = ros::Time::now();
00825         allowed_contact_tmp.pose_stamped.header.frame_id = contact_info_[i].header.frame_id;
00826         allowed_contact_tmp.link_names.push_back(contact_info_[i].contact_body_1);
00827         allowed_contact_tmp.penetration_depth = allowed_penetration_depth;
00828         allowed_contacts.push_back(allowed_contact_tmp);
00829         ROS_DEBUG_NAMED("manipulation","Added allowed contact region: %d",i);
00830         ROS_DEBUG_NAMED("manipulation","Position                    : (%f,%f,%f)",contact_info_[i].position.x,
00831                  contact_info_[i].position.y,contact_info_[i].position.z);
00832         ROS_DEBUG_NAMED("manipulation","Frame id                    : %s",contact_info_[i].header.frame_id.c_str());
00833         ROS_DEBUG_NAMED("manipulation","Depth                       : %f",allowed_penetration_depth);
00834         ROS_DEBUG_NAMED("manipulation","Link                        : %s",contact_info_[i].contact_body_1.c_str());
00835         ROS_DEBUG_NAMED("manipulation"," ");
00836       }
00837     }
00838     ROS_DEBUG_NAMED("manipulation","Added %d allowed contact regions",(int)allowed_contacts.size());
00839     move_arm_goal.planning_scene_diff.allowed_contacts = allowed_contacts;
00840   }
00841 }
00842 
00843 
00844 bool MechanismInterface::moveArmConstrained(std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose,
00845                                             const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00846                                             const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00847                                             const arm_navigation_msgs::Constraints &path_constraints,
00848                                             const double &redundancy,
00849                                             const bool &compute_viable_command_pose)
00850 {
00851   //make sure joint controllers are running
00852   if(!checkController(jointControllerName(arm_name)))
00853      switchToJoint(arm_name);
00854 
00855   int num_tries = 0;
00856   int max_tries = 1;
00857 
00858   arm_navigation_msgs::MoveArmGoal move_arm_goal;
00859   // no need to set the planning scene as move arm does not use that; instead
00860   // we'll set collision ops and link padding in move arm call
00861   move_arm_goal.planning_scene_diff.link_padding = link_padding;
00862   move_arm_goal.operations = collision_operations;
00863 
00864   move_arm_goal.motion_plan_request.group_name = handDescription().armGroup(arm_name)+"_cartesian";
00865   move_arm_goal.motion_plan_request.num_planning_attempts = 1;
00866   move_arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00867   move_arm_goal.motion_plan_request.planner_id = MOVE_ARM_PLANNER_ID;
00868   move_arm_goal.planner_service_name = MOVE_ARM_CONSTRAINED_PLANNER_SERVICE_NAME;
00869 
00870   move_arm_goal.motion_plan_request.goal_constraints.position_constraints.resize(1);
00871   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id = 
00872     commanded_pose.header.frame_id;
00873   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].link_name = 
00874     handDescription().gripperFrame(arm_name);
00875   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position = commanded_pose.pose.position;
00876   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type = 
00877     arm_navigation_msgs::Shape::BOX;
00878   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].
00879     constraint_region_shape.dimensions.push_back(OBJECT_POSITION_TOLERANCE_X);
00880   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].
00881     constraint_region_shape.dimensions.push_back(OBJECT_POSITION_TOLERANCE_Y);
00882   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].
00883     constraint_region_shape.dimensions.push_back(OBJECT_POSITION_TOLERANCE_Z);
00884   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00885   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00886   
00887   move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00888   move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0] = 
00889     path_constraints.orientation_constraints[0];
00890   move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00891 
00892   tf::Quaternion orientation;
00893   tf::quaternionMsgToTF(commanded_pose.pose.orientation,orientation);
00894   geometry_msgs::PoseStamped desired_pose = commanded_pose;  
00895   desired_pose.pose.position = move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position;
00896   desired_pose.header.stamp = ros::Time::now();
00897 
00898   // TODO - this should really happen through the constraint
00899   // Currently its hard-coded for keeping objects level
00900   if(compute_viable_command_pose && path_constraints.orientation_constraints.size())
00901   {
00902     bool get_ik = false;
00903     for(unsigned int i=0; i < 360; i++)
00904     {
00905       double rotation_yaw = i*M_PI/360.0;
00906       ROS_DEBUG_NAMED("manipulation","Trying to find suitable goal orientation with yaw rotation: %f",rotation_yaw);
00907       tf::Quaternion orientation_yaw;
00908       orientation_yaw.setRPY(0.0,0.0,rotation_yaw);
00909       orientation_yaw *= orientation;
00910       geometry_msgs::Quaternion quaternion;
00911       tf::quaternionTFToMsg(orientation_yaw,quaternion);
00912       desired_pose.pose.orientation = quaternion;
00913       desired_pose.header.stamp = ros::Time::now();
00914       kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
00915       if(getIKForPose(arm_name,desired_pose,ik_response, collision_operations, link_padding))
00916       {
00917         get_ik = true;
00918         break;
00919       }
00920     }
00921     if(!get_ik) 
00922       return false;
00923   }
00924 
00925   move_arm_goal.motion_plan_request.path_constraints.orientation_constraints.resize(1);
00926   move_arm_goal.motion_plan_request.path_constraints.orientation_constraints[0] = 
00927     move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0];
00928   move_arm_goal.disable_ik = true;
00929 
00930   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints.resize(1);
00931   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].joint_name = (getJointNames(arm_name))[2];
00932   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].position = redundancy;
00933   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].tolerance_below = M_PI;
00934   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].tolerance_above = M_PI;
00935       
00936   bool success = false;
00937   while(num_tries < max_tries)
00938   {
00939     move_arm_action_client_.client(arm_name).sendGoal(move_arm_goal);
00940     bool withinWait = move_arm_action_client_.client(arm_name).waitForResult(ros::Duration(60.0));
00941     if(!withinWait) 
00942     {
00943       move_arm_action_client_.client(arm_name).cancelGoal();
00944       ROS_DEBUG_NAMED("manipulation","  Move arm goal could not be achieved by move_arm in the allowed duration");
00945       success = false;
00946       num_tries++;
00947       arm_navigation_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00948       arm_navigation_msgs::ArmNavigationErrorCodes error_code;
00949       error_code.val = error_code.TIMED_OUT;
00950       modifyMoveArmGoal(move_arm_goal,error_code,move_arm_result.contacts);
00951       continue;
00952     }
00953     actionlib::SimpleClientGoalState state = move_arm_action_client_.client(arm_name).getState();
00954     if(state == actionlib::SimpleClientGoalState::SUCCEEDED) 
00955     {
00956       ROS_DEBUG_NAMED("manipulation","  Move arm: position was successfully achieved");
00957       success = true;
00958       break;
00959     } 
00960     else 
00961     {
00962       arm_navigation_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00963       ROS_DEBUG_NAMED("manipulation","Move arm: non-success state was reached. Reason: %s",
00964                (arm_navigation_msgs::armNavigationErrorCodeToString(move_arm_result.error_code)).c_str());
00965       ROS_DEBUG_NAMED("manipulation","num_tries: %d, max_tries: %d",num_tries,max_tries);
00966       success = false;
00967       num_tries++;
00968       modifyMoveArmGoal(move_arm_goal,move_arm_result.error_code,move_arm_result.contacts);
00969       continue;
00970     }
00971   }
00972   return success;
00973 }
00974 
00986 geometry_msgs::PoseStamped MechanismInterface::translateGripperPose(geometry_msgs::Vector3Stamped translation, 
00987                                                                     geometry_msgs::PoseStamped start_pose,
00988                                                                     std::string arm_name)
00989 {
00990   bool pre_multiply;
00991   if (translation.header.frame_id == handDescription().gripperFrame(arm_name))
00992   {
00993     pre_multiply=false;
00994   } 
00995   else if (translation.header.frame_id == handDescription().robotFrame(arm_name))
00996   {
00997     pre_multiply=true;
00998   }
00999   else
01000   {
01001     throw MechanismException(std::string("Gripper translations (such as for lift or approach) can only be specified in "
01002                                      "either the gripper frame or the robot frame. This one was specified in frame ") + 
01003                              translation.header.frame_id);
01004   }
01005 
01006 
01007   //go to robot frame first
01008   geometry_msgs::PoseStamped start_pose_robot_frame = transformPose(handDescription().robotFrame(arm_name), start_pose);
01009   tf::StampedTransform start_transform;
01010   tf::poseMsgToTF(start_pose_robot_frame.pose, start_transform);
01011 
01012   tf::Vector3 vec;
01013   tf::vector3MsgToTF(translation.vector, vec);
01014   tf::StampedTransform translate_trans;
01015   translate_trans.setIdentity();
01016   translate_trans.setOrigin(vec);
01017 
01018   //compute the translated pose
01019   tf::Transform end_transform;
01020   if (pre_multiply) end_transform = translate_trans * start_transform;
01021   else end_transform = start_transform * translate_trans;
01022 
01023   //prepare the results
01024   geometry_msgs::PoseStamped translated_pose;
01025   tf::poseTFToMsg(end_transform, translated_pose.pose);
01026   translated_pose.header.frame_id = handDescription().robotFrame(arm_name);
01027   translated_pose.header.stamp = ros::Time(0);
01028 
01029   //return the result in the requested frame
01030   translated_pose = transformPose(start_pose.header.frame_id, translated_pose);
01031 
01032   return translated_pose;
01033 }
01034 
01036 geometry_msgs::PoseStamped MechanismInterface::getGripperPose(std::string arm_name, std::string frame_id)
01037 {
01038   tf::StampedTransform gripper_transform;
01039   try
01040   {
01041     listener_.lookupTransform(frame_id, handDescription().gripperFrame(arm_name), ros::Time(0), gripper_transform);
01042   }
01043   catch (tf::TransformException ex)
01044   {
01045     //sometimes fails the first time but succeeds the second.  Don't ask me why.
01046     ROS_ERROR("failed to get tf transform for wrist roll link, trying a second time");
01047     try
01048     {
01049       listener_.lookupTransform(frame_id, handDescription().gripperFrame(arm_name), ros::Time(0), gripper_transform);
01050     }
01051     catch (tf::TransformException ex)
01052     {
01053       ROS_ERROR("Mechanism interface: failed to get tf transform for wrist roll link; tf exception %s", ex.what());
01054       throw MechanismException(std::string("failed to get tf transform for wrist roll link; tf exception: ") + 
01055                              std::string(ex.what()) );
01056     }
01057   }
01058   geometry_msgs::PoseStamped gripper_pose;
01059   tf::poseTFToMsg(gripper_transform, gripper_pose.pose);
01060   gripper_pose.header.frame_id = frame_id;
01061   gripper_pose.header.stamp = ros::Time::now();
01062   return gripper_pose;
01063 }
01064 
01065 void MechanismInterface::transformPointCloud(std::string target_frame, 
01066                                              const sensor_msgs::PointCloud &cloud_in,
01067                                              sensor_msgs::PointCloud &cloud_out)
01068 {
01069   try
01070   {
01071     listener_.transformPointCloud(target_frame, cloud_in, cloud_out);    
01072   }
01073   catch (tf::TransformException ex)
01074   {
01075     ROS_ERROR("Mechanism interface: failed to cloud into %s frame; exception: %s", target_frame.c_str(),
01076               ex.what());
01077     throw MechanismException(std::string("failed to transform cloud into frame ") + target_frame + 
01078                              std::string("; tf exception: ") + std::string(ex.what()) );
01079   }
01080 }
01081 
01082 void MechanismInterface::convertGraspableObjectComponentsToFrame(manipulation_msgs::GraspableObject &object,
01083                                                                  std::string frame_id)
01084 {
01085   if (!object.cluster.points.empty())
01086   {
01087     transformPointCloud(frame_id, object.cluster, object.cluster);
01088   }
01089   for (size_t i=0; i<object.potential_models.size(); i++)
01090   {
01091     object.potential_models[i].pose = transformPose(frame_id, object.potential_models[i].pose);
01092   }
01093   object.reference_frame_id = frame_id;
01094 }
01095 
01096 geometry_msgs::PoseStamped MechanismInterface::transformPose(const std::string target_frame, 
01097                                                              const geometry_msgs::PoseStamped &stamped_in)
01098 {
01099   geometry_msgs::PoseStamped stamped_out;
01100   try
01101   {
01102     listener_.transformPose(target_frame, stamped_in, stamped_out);
01103   }
01104   catch (tf::TransformException ex)
01105   {
01106     ROS_ERROR("Mechanism interface: failed to transform pose into %s frame; exception: %s", target_frame.c_str(),
01107               ex.what());
01108     throw MechanismException(std::string("failed to transform pose into frame ") + target_frame + 
01109                              std::string("; tf exception: ") + std::string(ex.what()) );
01110   }
01111   return stamped_out;
01112 }
01113 
01116 bool MechanismInterface::translateGripper(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
01117                                           arm_navigation_msgs::OrderedCollisionOperations ord, 
01118                                           const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
01119                                           float requested_distance, float min_distance,
01120                                           float &actual_distance)
01121 {
01122   //get the current gripper pose in the robot frame
01123   geometry_msgs::PoseStamped start_pose_stamped = getGripperPose(arm_name, handDescription().robotFrame(arm_name));
01124 
01125   //compute the interpolated trajectory
01126   trajectory_msgs::JointTrajectory traj;
01127   std::vector<double> empty;
01128   sensor_msgs::JointState empty_state;
01129   getInterpolatedIK(arm_name, 
01130                     start_pose_stamped, direction, requested_distance, 
01131                     empty, empty_state, ord, link_padding, false, traj, actual_distance);
01132 
01133   if (min_distance > 0 && actual_distance < min_distance) 
01134   {
01135     ROS_DEBUG_NAMED("manipulation","Mechanism interface: interpolated IK trajectory covers %f distance, but at least %f requested", 
01136              actual_distance, min_distance);
01137     actual_distance = 0;
01138     return false;
01139   }
01140 
01141   if (traj.points.empty())
01142   {
01143     ROS_DEBUG_NAMED("manipulation","Mechanism interface: translate gripper trajectory is empty");
01144     actual_distance = false;
01145     return false;
01146   }
01147 
01148   //execute the normalized interpolated trajectory
01149   attemptTrajectory(arm_name, traj, true);
01150   return true;
01151 }
01152 
01156 geometry_msgs::PoseStamped MechanismInterface::getObjectPoseForGrasp(std::string arm_name, 
01157                                                                      const geometry_msgs::Pose &grasp_pose)
01158 
01159 {
01160   //get the current pose of the gripper in base link coordinate frame
01161   tf::StampedTransform wrist_transform;
01162   try
01163   {
01164     listener_.lookupTransform("base_link",handDescription().gripperFrame(arm_name), 
01165                               ros::Time(0), wrist_transform);
01166   }
01167   catch (tf::TransformException ex)
01168   {
01169     ROS_ERROR("Mechanism interface: failed to get tf transform for wrist roll link");
01170     throw MechanismException(std::string("failed to get tf transform for wrist roll link; tf exception: ") + 
01171                              std::string(ex.what()) );
01172   }
01173 
01174   //multiply by inverse of grasp pose
01175   tf::Transform grasp_transform;
01176   tf::poseMsgToTF(grasp_pose, grasp_transform);
01177 
01178   tf::Transform object_transform;
01179   object_transform = wrist_transform * grasp_transform.inverse();
01180 
01181   //prepare the result
01182   geometry_msgs::PoseStamped object_pose;
01183   tf::poseTFToMsg(object_transform, object_pose.pose);
01184   object_pose.header.frame_id = "base_link";
01185   object_pose.header.stamp = ros::Time::now();
01186   return object_pose;
01187 }
01188 
01189 void MechanismInterface::attachObjectToGripper(std::string arm_name, std::string collision_object_name)
01190 {
01191   arm_navigation_msgs::AttachedCollisionObject obj;
01192   obj.object.header.stamp = ros::Time::now();
01193   obj.object.header.frame_id = handDescription().robotFrame(arm_name);
01194   obj.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT;
01195   obj.object.id = collision_object_name;
01196   obj.link_name = handDescription().attachLinkName(arm_name);
01197   obj.touch_links = handDescription().gripperTouchLinkNames(arm_name);
01198   attached_object_pub_.publish(obj);
01199 }
01200  
01201 void MechanismInterface::detachAndAddBackObjectsAttachedToGripper(std::string arm_name, 
01202                                                                   std::string collision_object_name)
01203 {
01204   arm_navigation_msgs::AttachedCollisionObject att;
01205   att.object.header.stamp = ros::Time::now();
01206   att.object.header.frame_id = handDescription().robotFrame(arm_name);
01207   att.link_name = handDescription().attachLinkName(arm_name);
01208   att.object.id = collision_object_name;
01209   att.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT;
01210   attached_object_pub_.publish(att);
01211 }
01212 
01213 void MechanismInterface::detachAllObjectsFromGripper(std::string arm_name)
01214 {
01215   arm_navigation_msgs::AttachedCollisionObject att;
01216   att.object.header.stamp = ros::Time::now();
01217   att.object.header.frame_id = handDescription().robotFrame(arm_name);
01218   att.link_name = handDescription().attachLinkName(arm_name);
01219   att.object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::REMOVE;
01220   attached_object_pub_.publish(att);
01221 }
01222 
01223 void MechanismInterface::handPostureGraspAction(std::string arm_name, 
01224                     const manipulation_msgs::Grasp &grasp, int goal, float max_contact_force)
01225 {
01226   object_manipulation_msgs::GraspHandPostureExecutionGoal posture_goal;
01227   posture_goal.grasp = grasp;
01228   posture_goal.goal = goal;
01229   posture_goal.max_contact_force = max_contact_force;
01230   hand_posture_client_.client(arm_name).sendGoal(posture_goal);
01231   bool withinWait = hand_posture_client_.client(arm_name).waitForResult(ros::Duration(10.0));
01232   if(!withinWait) 
01233   {
01234     hand_posture_client_.client(arm_name).cancelGoal();
01235     ROS_ERROR("Hand posture controller timed out on goal (%d)", goal);
01236     throw MechanismException("Hand posture controller timed out");
01237   }
01238   actionlib::SimpleClientGoalState state = hand_posture_client_.client(arm_name).getState();
01239   if(state != actionlib::SimpleClientGoalState::SUCCEEDED) 
01240   {
01241     ROS_ERROR("Hand posture controller failed on goal (%d)", goal);
01242     throw MechanismException("Hand posture controller failed");
01243   }
01244   ROS_DEBUG_NAMED("manipulation","Hand posture controller successfully achieved goal %d", goal);
01245 }
01246 
01247 bool MechanismInterface::graspPostureQuery(std::string arm_name, const manipulation_msgs::Grasp grasp)
01248 {
01249   object_manipulation_msgs::GraspStatus query;
01250   query.request.grasp = grasp;
01251   if (!grasp_status_client_.client(arm_name).call(query))
01252   {
01253     ROS_ERROR("Grasp posture query call failed");
01254     throw MechanismException("Grasp posture query call failed");
01255   }
01256   return query.response.is_hand_occupied;
01257 }
01258 
01259 bool MechanismInterface::pointHeadAction(const geometry_msgs::PointStamped &target, std::string pointing_frame, bool wait_for_result)
01260 {
01261   pr2_controllers_msgs::PointHeadGoal goal;
01262   goal.target = target;
01263   goal.pointing_axis.x = 0;
01264   goal.pointing_axis.y = 0;
01265   goal.pointing_axis.z = 1;
01266   goal.pointing_frame = pointing_frame;
01267   goal.min_duration = ros::Duration(0.05);
01268   goal.max_velocity = 1.0;
01269 
01270   point_head_action_client_.client().sendGoal(goal);
01271 
01272   if(wait_for_result)
01273   {
01274     point_head_action_client_.client().waitForResult( ros::Duration(3.0) );
01275 
01276     if (point_head_action_client_.client().getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01277     {
01278       ROS_DEBUG_NAMED("manipulation","Successfully moved head.");
01279       return true;
01280     }
01281     else
01282     {
01283       ROS_ERROR("Head movement failed or timed out.");
01284       return false;
01285     }
01286   }
01287   return true;
01288 }
01289 
01290 bool MechanismInterface::callSwitchControllers(std::vector<std::string> start_controllers, 
01291                                                std::vector<std::string> stop_controllers)
01292 {
01293   pr2_mechanism_msgs::SwitchController srv;
01294   srv.request.start_controllers = start_controllers;
01295   srv.request.stop_controllers = stop_controllers;
01296   srv.request.strictness = srv.request.STRICT;
01297   if ( !switch_controller_service_.client().call(srv) )
01298   {
01299     ROS_ERROR("Mechanism interface: switch controller service call failed");
01300     throw MechanismException("switch controller service call failed");
01301   } 
01302   if(!srv.response.ok) return false;
01303   return true;
01304 }  
01305 
01306 bool MechanismInterface::switchControllers(std::string start_controller, std::string stop_controller)
01307 {
01308   ROS_DEBUG_NAMED("manipulation","Switching controller %s for %s", start_controller.c_str(), stop_controller.c_str());
01309   std::vector<std::string> start_controllers;
01310   std::vector<std::string> stop_controllers;
01311   start_controllers.push_back(start_controller);
01312   stop_controllers.push_back(stop_controller);
01313   bool success = callSwitchControllers(start_controllers, stop_controllers);
01314   if(success)
01315   {
01316     bool start_running = checkController(start_controller);
01317     bool stop_running = checkController(stop_controller);
01318     if(start_running && !stop_running) return true;
01319     ROS_ERROR("switching %s to %s failed even though it returned success", 
01320               stop_controller.c_str(), start_controller.c_str());
01321     return false;
01322   }
01323   else
01324   {
01325     ROS_ERROR("switching %s to %s failed", stop_controller.c_str(), start_controller.c_str());
01326     return false;
01327   }
01328 }
01329 
01330 bool MechanismInterface::checkController(std::string controller)
01331 {
01332   pr2_mechanism_msgs::ListControllers srv;
01333   if( !list_controllers_service_.client().call(srv))
01334   {
01335     ROS_ERROR("Mechanism interface: list controllers service call failed");
01336     throw MechanismException("list controllers service call failed");
01337   }
01338   for(size_t controller_ind=0; controller_ind < srv.response.controllers.size(); controller_ind++)
01339   {
01340     if(controller.compare(srv.response.controllers[controller_ind]) == 0)
01341     {
01342       if(srv.response.state[controller_ind].compare("running") == 0) return true;
01343       return false;
01344     }
01345   }
01346   ROS_WARN("controller %s not found when checking status!", controller.c_str());
01347   return false;
01348 }
01349 
01350 bool MechanismInterface::startController(std::string controller)
01351 {
01352   ROS_DEBUG_NAMED("manipulation","Starting controller %s", controller.c_str());
01353   std::vector<std::string> start_controllers;
01354   std::vector<std::string> stop_controllers;
01355   start_controllers.push_back(controller);
01356   bool success = callSwitchControllers(start_controllers, stop_controllers);
01357   if(success)
01358   {
01359     bool running = checkController(controller);
01360     if(running) return true;
01361     ROS_ERROR("starting controller %s failed even though it returned success", controller.c_str());
01362     return false;
01363   }
01364   else
01365   {
01366     ROS_ERROR("starting controller %s failed", controller.c_str());
01367     return false;
01368   }
01369 }
01370 
01371 bool MechanismInterface::stopController(std::string controller)
01372 {
01373   ROS_DEBUG_NAMED("manipulation","Stopping controller %s", controller.c_str());
01374   std::vector<std::string> start_controllers;
01375   std::vector<std::string> stop_controllers;
01376   stop_controllers.push_back(controller);
01377   bool success = callSwitchControllers(start_controllers, stop_controllers);
01378   if(success)
01379   {
01380     bool running = checkController(controller);
01381     if(running)
01382     {
01383       ROS_ERROR("stopping controller %s failed even though it returned success", controller.c_str());
01384       return false;
01385     }
01386     return true;
01387   }
01388   else
01389   {
01390     ROS_ERROR("stopping controller %s failed", controller.c_str());
01391     return false;
01392   }
01393 }
01394 
01395 bool MechanismInterface::getArmAngles(std::string arm_name, std::vector<double> &arm_angles)
01396 {
01397   std::vector<std::string> arm_joints = handDescription().armJointNames(arm_name);
01398   if(arm_joints.size() == 0)
01399   {
01400     ROS_ERROR("mechanism interface: armJointNames was empty!");
01401     return false;
01402   }
01403   arm_angles.resize(arm_joints.size());
01404 
01405   //grab the latest JointState message
01406   arm_navigation_msgs::RobotState robot_state;
01407   getRobotState(robot_state);
01408   //find each joint and its current position in the JointState message
01409   for (size_t joint_num = 0; joint_num < arm_joints.size(); joint_num++)
01410   {
01411     std::string joint_name = arm_joints[joint_num];
01412     size_t i;
01413     for (i=0; i<robot_state.joint_state.name.size(); i++)
01414     {
01415       if (robot_state.joint_state.name[i] == joint_name) break;
01416     }
01417     if (i==robot_state.joint_state.name.size())
01418     {
01419       ROS_ERROR("mechanism interface: arm joint %s not found in joint state", joint_name.c_str());
01420       return false;
01421     }
01422     if (robot_state.joint_state.position.size() <= i)
01423     {
01424       ROS_ERROR("mechanism interface: malformed joint state message received");
01425       return false;
01426     }
01427     arm_angles[joint_num] = robot_state.joint_state.position[i];
01428   }
01429   ROS_DEBUG_NAMED("manipulation","%s arm_angles: %.2f %.2f %.2f %.2f %.2f %.2f %.2f",
01430            arm_name.c_str(), arm_angles[0], arm_angles[1], arm_angles[2], 
01431            arm_angles[3], arm_angles[4], arm_angles[5], arm_angles[6]);
01432   return true;
01433 }
01434 
01435 bool MechanismInterface::switchToCartesian(std::string arm_name)
01436 {
01437   //set the desired posture to the current joint angles first
01438   setCartesianPostureGoalToCurrentAngles(arm_name);
01439   
01440   //switch controllers
01441   bool result = switchControllers(cartesianControllerName(arm_name), jointControllerName(arm_name));
01442   if(!result) return false;
01443   return true;
01444 }
01445 
01446 bool MechanismInterface::switchToJoint(std::string arm_name)
01447 {
01448   bool result = switchControllers(jointControllerName(arm_name), cartesianControllerName(arm_name));
01449   if(!result) return false;
01450   return true;
01451 }
01452 
01453 std::string MechanismInterface::jointControllerName(std::string arm_name)
01454 {
01455   std::map<std::string, std::string>::iterator it = joint_controller_names_.find(arm_name);
01456   if ( it != joint_controller_names_.end() ) 
01457   {
01458     return it->second;
01459   }
01460   std::string controller_name;
01461   priv_nh_.param<std::string>(arm_name+std::string("_joint_controller"), controller_name, "");
01462   if(controller_name == "")
01463   {
01464     ROS_WARN("joint controller name for arm %s not found!", arm_name.c_str());
01465   }
01466   else
01467   {
01468     joint_controller_names_.insert(std::pair<std::string, std::string>(arm_name, controller_name));
01469     ROS_INFO("added controller name %s to joint controller map", controller_name.c_str());
01470   }
01471   return controller_name;
01472 }
01473 
01474 std::string MechanismInterface::cartesianControllerName(std::string arm_name)
01475 {
01476   std::map<std::string, std::string>::iterator it = cartesian_controller_names_.find(arm_name);
01477   if ( it != cartesian_controller_names_.end() ) 
01478   {
01479     return it->second;
01480   }
01481   std::string controller_name;
01482   priv_nh_.param<std::string>(arm_name+std::string("_cartesian_controller"), controller_name, "");
01483   if(controller_name == "")
01484   {
01485     ROS_WARN("Cartesian controller name for arm %s not found!", arm_name.c_str());
01486   }
01487   else
01488   {
01489     cartesian_controller_names_.insert(std::pair<std::string, std::string>(arm_name, controller_name));
01490     ROS_INFO("added controller name %s to Cartesian controller map", controller_name.c_str());
01491   }
01492   return controller_name;
01493 }
01494 
01495 geometry_msgs::PoseStamped MechanismInterface::clipDesiredPose(std::string arm_name,
01496                                                                const geometry_msgs::PoseStamped &desired_pose,
01497                                                                double clip_dist, double clip_angle,
01498                                                                double &resulting_clip_fraction)
01499 {
01500   //no clipping desired
01501   if(clip_dist == 0 && clip_angle == 0) return desired_pose;
01502 
01503   //Get the current gripper pose
01504   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01505 
01506   return clipDesiredPose(current_pose, desired_pose, clip_dist, clip_angle, resulting_clip_fraction);
01507 }
01508 
01509 geometry_msgs::PoseStamped MechanismInterface::clipDesiredPose(const geometry_msgs::PoseStamped &current_pose,
01510                                                                const geometry_msgs::PoseStamped &desired_pose,
01511                                                                double clip_dist, double clip_angle,
01512                                                                double &resulting_clip_fraction)
01513 {
01514   //no clipping desired
01515   if(clip_dist == 0 && clip_angle == 0) return desired_pose;
01516 
01517   //Get the position and angle dists between current and desired
01518   Eigen::Affine3d current_trans, desired_trans;
01519   double pos_dist, angle;
01520   Eigen::Vector3d axis, direction;
01521   tf::poseMsgToEigen(current_pose.pose, current_trans);
01522   tf::poseMsgToEigen(desired_pose.pose, desired_trans);
01523   positionAndAngleDist(current_trans, desired_trans, pos_dist, angle, axis, direction);
01524 
01525   //Clip the desired pose to be at most clip_dist and the desired angle to be at most clip_angle (proportional)
01526   //from the current
01527   double pos_mult, angle_mult;
01528   double pos_change, angle_change;
01529   angle_mult = fabs(angle / clip_angle);
01530   pos_mult = fabs(pos_dist / clip_dist);
01531   if(pos_mult <=1 && angle_mult <=1){
01532     return desired_pose;
01533   }
01534   double mult = pos_mult;
01535   if(angle_mult > pos_mult) mult = angle_mult;
01536   pos_change = pos_dist / mult;
01537   angle_change = angle / mult;
01538   resulting_clip_fraction = 1 / mult;
01539 
01540   Eigen::Affine3d clipped_trans;
01541   clipped_trans = current_trans;
01542   Eigen::Vector3d scaled_direction;
01543   scaled_direction = direction * pos_change;
01544   Eigen::Translation3d translation(scaled_direction);
01545   clipped_trans = clipped_trans * translation;
01546   Eigen::AngleAxis<double> angle_axis(angle_change, axis);
01547   clipped_trans = clipped_trans * angle_axis;
01548   geometry_msgs::PoseStamped clipped_pose;
01549   tf::poseEigenToMsg(clipped_trans, clipped_pose.pose);
01550   clipped_pose.header = desired_pose.header;
01551   return clipped_pose;
01552 }
01553 
01554 
01555 geometry_msgs::PoseStamped MechanismInterface::overshootDesiredPose(std::string arm_name, 
01556                                                                     const geometry_msgs::PoseStamped &desired_pose, 
01557                                                                     double overshoot_dist, double overshoot_angle,
01558                                                                     double dist_tol, double angle_tol)
01559 {
01560   //Get the current gripper pose
01561   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01562 
01563   //Get the position and angle dists between current and desired
01564   Eigen::Affine3d current_trans, desired_trans;
01565   double pos_dist, angle;
01566   Eigen::Vector3d axis, direction;
01567   tf::poseMsgToEigen(current_pose.pose, current_trans);
01568   tf::poseMsgToEigen(desired_pose.pose, desired_trans);
01569   positionAndAngleDist(current_trans, desired_trans, pos_dist, angle, axis, direction);
01570 
01571   //Add overshoot_dist in length to the pos difference vector, and overshoot_angle to the angle difference
01572   double pos_change = 0;
01573   double angle_change = 0;
01574   if(angle > angle_tol) angle_change = angle + overshoot_angle;
01575   double pos_sign = pos_dist / fabs(pos_dist);
01576   if(fabs(pos_dist) > dist_tol) pos_change = pos_dist + pos_sign*overshoot_dist;
01577   //std::cout << "pos_change: " << pos_change << "angle_change: " << angle_change;
01578 
01579   Eigen::Affine3d overshoot_trans;
01580   overshoot_trans = current_trans;
01581   Eigen::Vector3d scaled_direction;
01582   scaled_direction = direction * pos_change;
01583   Eigen::Translation3d translation(scaled_direction);
01584   overshoot_trans = overshoot_trans * translation;
01585   Eigen::AngleAxis<double> angle_axis(angle_change, axis);
01586   overshoot_trans = overshoot_trans * angle_axis;
01587   geometry_msgs::PoseStamped overshoot_pose;
01588   tf::poseEigenToMsg(overshoot_trans, overshoot_pose.pose);
01589   overshoot_pose.header = desired_pose.header;
01590 
01591   return overshoot_pose;
01592 } 
01593 
01594 
01595 geometry_msgs::PoseStamped MechanismInterface::clipDesiredPose(std::string arm_name, 
01596                                                                const geometry_msgs::PoseStamped &desired_pose, 
01597                                                                double clip_dist, double clip_angle,
01598                                                                double &resulting_clip_fraction,
01599                                                                const std::vector<double> &goal_posture_suggestion,
01600                                                                std::vector<double> &clipped_posture_goal)
01601 {
01602   double clip_fraction;
01603   geometry_msgs::PoseStamped clipped_pose = clipDesiredPose(arm_name, desired_pose, clip_dist, clip_angle, clip_fraction);
01604 
01605   //If there's a goal posture suggestion, clip it as well
01606   if(goal_posture_suggestion.size() > 0 && (clip_dist != 0 || clip_angle != 0))
01607   {
01608     //Get the current arm angles
01609     std::vector<double> current_arm_angles;
01610     getArmAngles(arm_name, current_arm_angles);
01611 
01612     if(goal_posture_suggestion.size() != current_arm_angles.size())
01613     {
01614       ROS_ERROR("goal posture suggestion length not consistent with length of current arm angles!");
01615       return clipped_pose;
01616     }
01617 
01618     //Unnormalize the goal arm angles
01619     std::vector<std::vector<double> > goal_positions;
01620     goal_positions.push_back(goal_posture_suggestion);
01621     trajectory_msgs::JointTrajectory goal_trajectory = assembleJointTrajectory(arm_name, goal_positions, 5.0);
01622     goal_trajectory.header.stamp = ros::Time(0);
01623     trajectory_msgs::JointTrajectory unnormalized_goal;
01624     unnormalizeTrajectory(arm_name, goal_trajectory, unnormalized_goal);
01625 
01626     //Clip the posture goal by the same amount the pose got clipped
01627     clipped_posture_goal.resize(current_arm_angles.size());
01628     for(size_t i=0; i<current_arm_angles.size(); i++)
01629     {
01630       clipped_posture_goal[i] = 
01631         (unnormalized_goal.points[0].positions[i] - current_arm_angles[i]) * clip_fraction + current_arm_angles[i];
01632     }
01633   }
01634 
01635   return clipped_pose;
01636 }
01637 
01638 void MechanismInterface::poseDists(geometry_msgs::Pose start, geometry_msgs::Pose end, double &pos_dist, double &angle)
01639 {
01640   Eigen::Affine3d start_trans, end_trans;
01641   tf::poseMsgToEigen(start, start_trans);
01642   tf::poseMsgToEigen(end, end_trans);
01643   Eigen::Vector3d axis, direction;
01644   positionAndAngleDist(start_trans, end_trans, pos_dist, angle, axis, direction);
01645 }
01646 
01647 void MechanismInterface::positionAndAngleDist(Eigen::Affine3d start, Eigen::Affine3d end, 
01648                                               double &pos_dist, 
01649                                               double &angle, Eigen::Vector3d &axis, Eigen::Vector3d &direction)
01650 {
01651   //trans = end to start = global to start * end to global
01652   Eigen::Affine3d trans;
01653   trans = start.inverse() * end;
01654   Eigen::AngleAxis<double> angle_axis;
01655   angle_axis = trans.rotation();
01656   angle = angle_axis.angle();
01657   axis = angle_axis.axis();
01658   if(angle > M_PI)
01659   {
01660     angle = -(angle - 2*M_PI);
01661     axis = -axis;
01662   }
01663   direction = trans.translation();
01664   pos_dist = sqrt(direction.dot(direction));
01665   if(pos_dist) direction *= 1/pos_dist;
01666 }
01667 
01668 int MechanismInterface::translateGripperCartesian(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
01669                                                   ros::Duration timeout, double dist_tol, double angle_tol,
01670                                                   double clip_dist, double clip_angle, 
01671                                                   double overshoot_dist, double overshoot_angle, double timestep)
01672 {
01673   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, direction.header.frame_id);
01674   geometry_msgs::PoseStamped desired_pose = translateGripperPose(direction, current_pose, arm_name);  
01675   int result = moveArmToPoseCartesian(arm_name, desired_pose, timeout, dist_tol, angle_tol, 
01676                                       clip_dist, clip_angle, overshoot_dist, overshoot_angle, timestep);
01677   return result;
01678 }
01679 
01680 //returns 0 if an error occurred, 1 if it got there, -1 if it timed out
01681 int MechanismInterface::moveArmToPoseCartesian(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
01682                                                ros::Duration timeout, double dist_tol, double angle_tol,
01683                                                double clip_dist, double clip_angle, 
01684                                                double overshoot_dist, double overshoot_angle, 
01685                                                double timestep, 
01686                                                const std::vector<double> &goal_posture_suggestion)
01687 {
01688   bool success = false;
01689 
01690   //Switch to Cartesian controllers
01691   for(int tries=0; tries<3; tries++)
01692   {
01693     success = switchToCartesian(arm_name);
01694     if(success) break;
01695     ros::Duration(1.0).sleep();
01696   }
01697   if(!success)
01698   {
01699     ROS_ERROR("Tries exceeded when trying to switch to Cartesian control!");
01700     return 0;
01701   }
01702 
01703   //Move towards the desired pose until we get there within our tolerance or until time runs out
01704   ros::Time begin = ros::Time::now();
01705   int reached_target = -1;
01706   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01707   geometry_msgs::PoseStamped last_pose = current_pose;
01708 
01709   //Compute a new desired_pose with the desired overshoot
01710   geometry_msgs::PoseStamped overshoot_pose = overshootDesiredPose(arm_name, desired_pose, overshoot_dist, overshoot_angle, dist_tol, angle_tol);
01711 
01712   while(ros::ok())
01713   {
01714     //stop if we're out of time
01715     if(ros::Time::now() - begin > timeout)
01716     {
01717       ROS_DEBUG_NAMED("manipulation","Timed out when moving to desired Cartesian pose");
01718       break;
01719     }
01720     
01721     //stop if we're within our tolerances
01722     current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01723 
01724     double pos_dist, angle_dist;
01725     poseDists(current_pose.pose, overshoot_pose.pose, pos_dist, angle_dist);
01726     if(pos_dist <= dist_tol+overshoot_dist && angle_dist <= angle_tol+overshoot_angle)
01727     {
01728       //send the original desired pose out
01729       sendCartesianPoseCommand(arm_name, desired_pose);
01730 
01731       //ROS_INFO("moveArmToPoseCartesian reached target");
01732       reached_target = 1;
01733       break;
01734     }
01735 
01736     //also stop if we're not moving
01737     double pos_change, angle_change;
01738     poseDists(current_pose.pose, last_pose.pose, pos_change, angle_change);
01739     if(ros::Time::now() - begin > ros::Duration(2.0) && pos_change <= .005*timestep && angle_change <= .001*timestep)
01740     {
01741       ROS_INFO("moveArmToPoseCartesian is stuck!  Returning");
01742       break;
01743     }
01744      
01745     //clip the desired pose and posture suggestion and send it out
01746     double resulting_clip_fraction;
01747     std::vector<double> clipped_posture_suggestion;
01748     geometry_msgs::PoseStamped clipped_pose = clipDesiredPose(arm_name, overshoot_pose, clip_dist, clip_angle, 
01749                                     resulting_clip_fraction, goal_posture_suggestion, clipped_posture_suggestion);
01750     sendCartesianPoseCommand(arm_name, clipped_pose);
01751 
01752     if(clipped_posture_suggestion.size() > 0)
01753     {
01754       sendCartesianPostureCommand(arm_name, clipped_posture_suggestion);
01755     }
01756 
01757     last_pose = current_pose;
01758 
01759     //sleep until the next timestep
01760     ros::Duration(timestep).sleep();
01761   }
01762 
01763   //Switch back to joint control
01764   success = false;
01765   for(int tries=0; tries<3; tries++)
01766   {
01767     success = switchToJoint(arm_name);
01768     if(success) break;
01769     ros::Duration(1.0).sleep();
01770   }
01771   if(!success)
01772   {
01773     ROS_ERROR("Tries exceeding when trying to switch back to joint control!");
01774     return 0;
01775   }
01776   return reached_target;
01777 }
01778 
01779 void MechanismInterface::sendCartesianPoseCommand(std::string arm_name, geometry_msgs::PoseStamped desired_pose)
01780 {
01781   desired_pose.header.stamp = ros::Time(0);
01782   cartesian_pub_.publisher(arm_name).publish(desired_pose);
01783 }
01784 
01785 void MechanismInterface::sendCartesianPostureCommand(std::string arm_name, std::vector<double> arm_angles)
01786 {
01787   std_msgs::Float64MultiArray angles;
01788   for(size_t i=0; i<arm_angles.size(); i++)
01789   {
01790     angles.data.push_back(arm_angles[i]);
01791   }
01792   cartesian_posture_pub_.publisher(arm_name).publish(angles);
01793 }
01794 
01795 /*
01796 void MechanismInterface::sendCartesianGainsCommand(std::string arm_name, std::vector<double> gains)
01797 {
01798   std_msgs::Float64MultiArray new_gains;
01799   new_gains.data = gains;
01800   cartesian_gains_pub_.publisher(arm_name).publish(new_gains);
01801   }*/
01802  /*
01803 void MechanismInterface::sendCartesianGainsCommand(std::string arm_name, std::vector<double> gains, std::string frame_id, std::vector<double> fixed_frame)
01804 {
01805   object_manipulation_msgs::CartesianGains new_gains;
01806   new_gains.gains = gains;
01807   new_gains.fixed_frame = fixed_frame;
01808   new_gains.header.frame_id = frame_id;
01809   cartesian_gains_pub_.publisher(arm_name).publish(new_gains);
01810   }*/
01811 
01812 void MechanismInterface::setCartesianPostureGoalToCurrentAngles(std::string arm_name)
01813 {
01814   std::vector<double> arm_angles;
01815   getArmAngles(arm_name, arm_angles);
01816   sendCartesianPostureCommand(arm_name, arm_angles);
01817 }
01818 
01819 std::vector<arm_navigation_msgs::LinkPadding> 
01820 MechanismInterface::fingertipPadding(std::string arm_name, double pad)
01821 {
01822   std::vector<arm_navigation_msgs::LinkPadding> padding_vec;
01823   arm_navigation_msgs::LinkPadding padding;
01824   padding.padding = pad;  
01825   std::vector<std::string> links = handDescription().fingertipLinks(arm_name);
01826   for (size_t i=0; i<links.size(); i++)
01827   {
01828     padding.link_name = links[i];
01829     padding_vec.push_back(padding); 
01830   }  
01831   return padding_vec; 
01832 }
01833 
01834 std::vector<arm_navigation_msgs::LinkPadding> 
01835 MechanismInterface::gripperPadding(std::string arm_name, double pad)
01836 {
01837   std::vector<arm_navigation_msgs::LinkPadding> padding_vec;
01838   arm_navigation_msgs::LinkPadding padding;
01839   padding.padding = pad;  
01840   std::vector<std::string> links = handDescription().gripperTouchLinkNames(arm_name);
01841   for (size_t i=0; i<links.size(); i++)
01842   {
01843     padding.link_name = links[i];
01844     padding_vec.push_back(padding); 
01845   }  
01846   return padding_vec; 
01847 }
01848 
01849 } //namespace object_manipulator


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