katana_object_manipulator: katana_object_manipulator: mechanism

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 namespace object_manipulator {
00039 
00040 static const std::string IK_SERVICE_SUFFIX = "/constraint_aware_ik";
00041 static const std::string FK_SERVICE_SUFFIX = "/get_fk";
00042 static const std::string INTERPOLATED_IK_SERVICE_SUFFIX = "/interpolated_ik";
00043 static const std::string INTERPOLATED_IK_SET_PARAMS_SERVICE_SUFFIX = "/interpolated_ik_set_params";
00044 static const std::string IK_QUERY_SERVICE_SUFFIX = "/get_ik_solver_info";
00045 static const std::string GRASP_STATUS_SUFFIX = "/grasp_status";
00046 
00047 static const std::string CHECK_STATE_VALIDITY_NAME = "environment_server/get_state_validity";
00048 static const std::string NORMALIZE_SERVICE_NAME = "trajectory_filter_unnormalizer/filter_trajectory";
00049 
00050 static const std::string REACTIVE_GRASP_ACTION_SUFFIX = "/reactive_grasp";
00051 static const std::string REACTIVE_LIFT_ACTION_SUFFIX = "/reactive_lift";
00052 static const std::string REACTIVE_PLACE_ACTION_SUFFIX = "/reactive_place";
00053 static const std::string MOVE_ARM_ACTION_SUFFIX = "/move_arm";
00054 static const std::string TRAJECTORY_ACTION_SUFFIX = "/joint_trajectory";
00055 static const std::string HAND_POSTURE_ACTION_SUFFIX = "/hand_posture_execution";
00056 
00057 static const std::string SWITCH_CONTROLLER_SERVICE_NAME = "/switch_controller";
00058 static const std::string LIST_CONTROLLERS_SERVICE_NAME = "/list_controllers";
00059 static const std::string CARTESIAN_COMMAND_SUFFIX = "/cart/command_pose";
00060 
00061 static const std::string MOVE_ARM_PLANNER_ID = "SBLkConfig1";
00062 static const std::string MOVE_ARM_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path";
00063 static const std::string MOVE_ARM_CONSTRAINED_PLANNER_SERVICE_NAME = "ompl_planning/plan_kinematic_path";
00064 
00065 static const std::string ATTACHED_COLLISION_TOPIC="attached_collision_object";
00066 
00067 static const std::string POINT_HEAD_ACTION_TOPIC = "/head_traj_controller/point_head_action";
00068 
00069 static const double OBJECT_POSITION_TOLERANCE_X = 0.02;
00070 static const double OBJECT_POSITION_TOLERANCE_Y = 0.02;
00071 static const double OBJECT_POSITION_TOLERANCE_Z = 0.02;
00072 
00073 MechanismInterface::MechanismInterface() :
00074   root_nh_(""),priv_nh_("~"),
00075 
00076   //------------------- multi arm service clients -----------------------
00077   ik_query_client_("", IK_QUERY_SERVICE_SUFFIX, true),
00078   ik_service_client_("", IK_SERVICE_SUFFIX, true),
00079   fk_service_client_("",FK_SERVICE_SUFFIX,true),
00080   interpolated_ik_service_client_("", INTERPOLATED_IK_SERVICE_SUFFIX, true),
00081   interpolated_ik_set_params_client_("", INTERPOLATED_IK_SET_PARAMS_SERVICE_SUFFIX, true),
00082   grasp_status_client_("", GRASP_STATUS_SUFFIX, true),
00083   //------------------- simple service clients -----------------------
00084   check_state_validity_client_(CHECK_STATE_VALIDITY_NAME),
00085   joint_trajectory_normalizer_service_(NORMALIZE_SERVICE_NAME),
00086   switch_controller_service_(SWITCH_CONTROLLER_SERVICE_NAME),
00087   list_controllers_service_(LIST_CONTROLLERS_SERVICE_NAME),
00088   //-------------------- multi arm action clients -----------------------
00089   reactive_grasp_action_client_("", REACTIVE_GRASP_ACTION_SUFFIX, true, true),
00090   reactive_lift_action_client_("", REACTIVE_LIFT_ACTION_SUFFIX, true, true),
00091   reactive_place_action_client_("", REACTIVE_PLACE_ACTION_SUFFIX, true, true),
00092   move_arm_action_client_("", MOVE_ARM_ACTION_SUFFIX, true, true),
00093   traj_action_client_("", TRAJECTORY_ACTION_SUFFIX, true, true),
00094   hand_posture_client_("", HAND_POSTURE_ACTION_SUFFIX, true, true),
00095   //-------------------- head action client -----------------------
00096   point_head_action_client_(POINT_HEAD_ACTION_TOPIC, true)
00097 {
00098   //collision map publishing topic
00099   attached_object_pub_ = root_nh_.advertise<mapping_msgs::AttachedCollisionObject>(ATTACHED_COLLISION_TOPIC, 10);
00100 
00101   //Cartesian pose command publishing topics
00102   right_cartesian_pub_ = root_nh_.advertise<geometry_msgs::PoseStamped>(std::string("right_arm")+
00103                                                                         CARTESIAN_COMMAND_SUFFIX, 100);
00104   left_cartesian_pub_ = root_nh_.advertise<geometry_msgs::PoseStamped>(std::string("left_arm")+
00105                                                                        CARTESIAN_COMMAND_SUFFIX, 100);
00106 
00107   //controller names
00108   priv_nh_.param<std::string>("right_cartesian_controller", right_cartesian_controller_, "r_cart");
00109   priv_nh_.param<std::string>("left_cartesian_controller", left_cartesian_controller_, "l_cart");
00110   priv_nh_.param<std::string>("right_joint_controller", right_joint_controller_, "r_arm_controller");
00111   priv_nh_.param<std::string>("left_joint_controller", left_joint_controller_, "l_arm_controller");
00112 }
00113 
00117 std::vector<std::string> MechanismInterface::getJointNames(std::string arm_name)
00118 {
00119   kinematics_msgs::GetKinematicSolverInfo::Request query_request;
00120   kinematics_msgs::GetKinematicSolverInfo::Response query_response;
00121   if ( !ik_query_client_.client(arm_name).call(query_request, query_response) )
00122   {
00123     ROS_ERROR("Failed to call ik information query");
00124     throw MechanismException("Failed to call ik information query");
00125   }
00126   return query_response.kinematic_solver_info.joint_names;
00127 }
00128 
00129 void MechanismInterface::attemptTrajectory(std::string arm_name,
00130                                            const std::vector< std::vector<double> > &positions,
00131                                            bool unnormalize,
00132                                            float time_per_segment)
00133 {
00134   trajectory_msgs::JointTrajectory trajectory;
00135   trajectory.header.frame_id = handDescription().robotFrame(arm_name);
00136   trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00137   trajectory.joint_names = getJointNames(arm_name);
00138   float current_time = 0;
00139   for (size_t i=0; i<positions.size(); i++)
00140   {
00141     current_time += time_per_segment;
00142     if (positions[i].size() != trajectory.joint_names.size())
00143     {
00144       ROS_ERROR("Mechanism interface: requested trajectory does not have enough joint values");
00145       throw MechanismException("Mechanism interface: requested trajectory does not have enough joint values");
00146     }
00147     trajectory_msgs::JointTrajectoryPoint point;
00148     point.positions = positions[i];
00149     point.time_from_start = ros::Duration(current_time);
00150     trajectory.points.push_back(point);
00151   }
00152   attemptTrajectory(arm_name, trajectory, unnormalize);
00153 }
00154 
00155 void MechanismInterface::attemptTrajectory(std::string arm_name,
00156                                            const trajectory_msgs::JointTrajectory &trajectory,
00157                                            bool unnormalize)
00158 {
00159   if (trajectory.points.empty())
00160   {
00161     ROS_ERROR("attemptTrajectory called with empty trajectory");
00162     throw MechanismException("attemptTrajectory called with empty trajectory");
00163   }
00164 
00165   //make sure joint controllers are running
00166   if(!checkController(right_joint_controller_) || !checkController(left_joint_controller_))
00167      switchToJoint();
00168 
00169   pr2_controllers_msgs::JointTrajectoryGoal goal;
00170   if (unnormalize)
00171   {
00172     motion_planning_msgs::FilterJointTrajectory service_call;
00173     service_call.request.trajectory = trajectory;
00174     service_call.request.allowed_time = ros::Duration(2.0);
00175     if ( !joint_trajectory_normalizer_service_.client().call(service_call) )
00176     {
00177       ROS_ERROR("Mechanism interface: joint trajectory normalizer service call failed");
00178       throw MechanismException("joint trajectory normalizer service call failed");
00179     }
00180     goal.trajectory = service_call.response.trajectory;
00181   }
00182   else
00183   {
00184     goal.trajectory = trajectory;
00185   }
00186   //force it to wait for client here, so that the duration below is not wasted
00187   traj_action_client_.client(arm_name);
00188   goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(1.0);
00189 
00190   //wait 5 seconds more that the whole trajectory is supposed to take
00191   ros::Duration timeout = ros::Duration(1.0) + trajectory.points.back().time_from_start + ros::Duration(5.0);
00192   traj_action_client_.client(arm_name).sendGoal(goal);
00193   if ( !traj_action_client_.client(arm_name).waitForResult(timeout) )
00194   {
00195     ROS_ERROR("  Trajectory timed out");
00196     throw MechanismException("trajectory timed out");
00197   }
00198 }
00199 
00200 void MechanismInterface::setInterpolatedIKParams(std::string arm_name, int num_steps,
00201                                                  int collision_check_resolution, bool start_from_end)
00202 {
00203   interpolated_ik_motion_planner::SetInterpolatedIKMotionPlanParams srv;
00204   srv.request.num_steps = num_steps;
00205   srv.request.consistent_angle = M_PI/6;
00206   srv.request.collision_check_resolution = collision_check_resolution;
00207   srv.request.steps_before_abort = -1;
00208   srv.request.pos_spacing = 0.01; //ignored if num_steps !=0
00209   srv.request.rot_spacing = 0.1;  //ignored if num_steps !=0
00210   srv.request.collision_aware = true;
00211   srv.request.start_from_end = start_from_end;
00212   if (!interpolated_ik_set_params_client_.client(arm_name).call(srv))
00213   {
00214     ROS_ERROR("Failed to set Interpolated IK server parameters");
00215     throw MechanismException("Failed to set Interpolated IK server parameters");
00216   }
00217 }
00218 
00219 bool MechanismInterface::moveArmToPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00220                                        const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
00221                                        const std::vector<motion_planning_msgs::LinkPadding> &link_padding)
00222 {
00223   kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
00224   if(!getIKForPose(arm_name, desired_pose,ik_response, collision_operations, link_padding))
00225     return false;
00226   if(!attemptMoveArmToGoal(arm_name, ik_response.solution.joint_state.position, collision_operations, link_padding))
00227     return false;
00228   return true;
00229 }
00230 
00231 bool MechanismInterface::getFK(std::string arm_name,
00232                                std::vector<double> positions,
00233                                geometry_msgs::PoseStamped &pose_stamped)
00234 {
00235  // define the service messages
00236  kinematics_msgs::GetPositionFK::Request  fk_request;
00237  kinematics_msgs::GetPositionFK::Response fk_response;
00238  fk_request.header.frame_id = pose_stamped.header.frame_id;
00239  fk_request.header.stamp = pose_stamped.header.stamp;
00240  fk_request.fk_link_names.resize(1);
00241  fk_request.fk_link_names[0] = handDescription().gripperFrame(arm_name);
00242  fk_request.robot_state.joint_state.position = positions;
00243  fk_request.robot_state.joint_state.name = getJointNames(arm_name);
00244  if( !fk_service_client_.client(arm_name).call(fk_request,fk_response) )
00245    {
00246      ROS_ERROR("FK Service Call failed altogether");
00247      throw MechanismException("FK Service Call failed altogether");
00248    }
00249  if (fk_response.error_code.val != fk_response.error_code.SUCCESS)
00250    {
00251     ROS_ERROR("Get FK failed with error code %d", fk_response.error_code.val);
00252     return false;
00253    }
00254  pose_stamped = fk_response.pose_stamped[0];
00255  return true;
00256 }
00257 
00258 bool MechanismInterface::getIKForPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00259                                       kinematics_msgs::GetConstraintAwarePositionIK::Response& ik_response,
00260                                       const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
00261                                       const std::vector<motion_planning_msgs::LinkPadding> &link_padding)
00262 {
00263   //call collision-aware ik
00264   kinematics_msgs::GetConstraintAwarePositionIK::Request ik_request;
00265   ik_request.ik_request.ik_link_name = handDescription().gripperFrame(arm_name);
00266   ik_request.ik_request.pose_stamped.pose = desired_pose.pose;
00267   ik_request.ik_request.pose_stamped.header.stamp = desired_pose.header.stamp;
00268   ik_request.ik_request.pose_stamped.header.frame_id = desired_pose.header.frame_id;
00269   ik_request.ik_request.ik_seed_state.joint_state.name = getJointNames(arm_name);
00270   ik_request.ik_request.ik_seed_state.joint_state.position.resize(5, 0.0);
00271   ik_request.ordered_collision_operations = collision_operations;
00272   ik_request.link_padding = link_padding;
00273   ik_request.timeout = ros::Duration(2.0);
00274   if( !ik_service_client_.client(arm_name).call(ik_request,ik_response) )
00275   {
00276     ROS_ERROR("IK Service Call failed altogether");
00277     throw MechanismException("IK Service Call failed altogether");
00278   }
00279   if (ik_response.error_code.val != ik_response.error_code.SUCCESS)
00280   {
00281     ROS_ERROR("Get IK failed with error code %d", ik_response.error_code.val);
00282     return false;
00283   }
00284   return true;
00285 }
00286 
00287 bool MechanismInterface::checkStateValidity(std::string arm_name, const std::vector<double> &joint_values,
00288                                           const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
00289                                             const std::vector<motion_planning_msgs::LinkPadding> &link_padding)
00290 {
00291   planning_environment_msgs::GetStateValidity::Request req;
00292   planning_environment_msgs::GetStateValidity::Response res;
00293 
00294   req.robot_state.joint_state.name = getJointNames(arm_name);
00295   req.robot_state.joint_state.position = joint_values;
00296   if ( req.robot_state.joint_state.name.size() != joint_values.size() )
00297   {
00298     throw MechanismException("Wrong number of joint values for checkStateValidity");
00299   }
00300   req.robot_state.joint_state.header.stamp = ros::Time::now();
00301   req.check_collisions = true;
00302   req.ordered_collision_operations = collision_operations;
00303   req.link_padding = link_padding;
00304 
00305   if(!check_state_validity_client_.client().call(req,res))
00306   {
00307     throw MechanismException("Call to check state validity client failed");
00308   }
00309 
00310   if (res.error_code.val == res.error_code.SUCCESS) return true;
00311   return false;
00312 }
00313 
00326 int MechanismInterface::getInterpolatedIK(std::string arm_name,
00327                                           geometry_msgs::PoseStamped start_pose,
00328                                           geometry_msgs::Vector3Stamped direction,
00329                                           float desired_trajectory_length,
00330                                           const std::vector<double> &seed_joint_position,
00331                                           const sensor_msgs::JointState &joint_state,
00332                                           const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
00333                                           const std::vector<motion_planning_msgs::LinkPadding> &link_padding,
00334                                           bool reverse_trajectory,
00335                                           trajectory_msgs::JointTrajectory &trajectory,
00336                                           float &actual_trajectory_length)
00337 {
00338   //first compute the desired end pose
00339   //make sure the input is normalized
00340   geometry_msgs::Vector3Stamped direction_norm = direction;
00341   direction_norm.vector = normalize(direction.vector);
00342   //multiply by the length
00343   desired_trajectory_length = fabs(desired_trajectory_length);
00344   direction_norm.vector.x *= desired_trajectory_length;
00345   direction_norm.vector.y *= desired_trajectory_length;
00346   direction_norm.vector.z *= desired_trajectory_length;
00347   ROS_INFO("translate from %f, %f , %f with vec: %f, %f, %f", start_pose.pose.position.x, start_pose.pose.position.y, start_pose.pose.position.z, direction_norm.vector.x,  direction_norm.vector.y,  direction_norm.vector.z);
00348 
00349   geometry_msgs::PoseStamped end_pose = translateGripperPose(direction_norm, start_pose, arm_name);
00350 
00351   ROS_INFO("computed end pose: %f, %f, %f", end_pose.pose.position.x, end_pose.pose.position.y, end_pose.pose.position.z);
00352   //hard-coded for now
00353   float max_step_size = 0.01;
00354   unsigned int collision_check_resolution = 1;
00355 
00356   //compute the number of steps
00357   unsigned int num_steps = (unsigned int)ceil(desired_trajectory_length / fabs(max_step_size));
00358   float actual_step_size = desired_trajectory_length / num_steps;
00359 
00360   ROS_INFO("Trajectory details: length %f, requested num steps: %d, actual step size: %f",
00361            desired_trajectory_length, num_steps, actual_step_size);
00362 
00363   if (reverse_trajectory)
00364   {
00365     std::swap(start_pose, end_pose);
00366   }
00367 
00368   //recall that here we setting the numbre of points in trajectory, which is steps+1
00369   setInterpolatedIKParams(arm_name, num_steps+1, collision_check_resolution, reverse_trajectory);
00370 
00371   motion_planning_msgs::RobotState start_state;
00372   start_state.multi_dof_joint_state.child_frame_ids.push_back(handDescription().gripperFrame(arm_name));
00373   start_state.multi_dof_joint_state.poses.push_back(start_pose.pose);
00374   start_state.multi_dof_joint_state.frame_ids.push_back(start_pose.header.frame_id);
00375   start_state.multi_dof_joint_state.stamp = ros::Time::now();
00376 
00377   //pass the seeds for the IK, if any
00378   if (!seed_joint_position.empty())
00379   {
00380     //we are silently assuming that the values passed in match out joint names for IK
00381     start_state.joint_state.name = getJointNames(arm_name);
00382     if (seed_joint_position.size() != start_state.joint_state.name.size())
00383     {
00384       ROS_ERROR("Interpolated IK request: seed_joint_position does not match joint names");
00385       throw MechanismException("Interpolated IK request: seed_joint_position does not match joint names");
00386     }
00387     start_state.joint_state.position = seed_joint_position;
00388   }
00389 
00390   //pass the desired values of non-planned joints, if any
00391   for (size_t i=0; i<joint_state.name.size(); i++)
00392   {
00393     start_state.joint_state.name.push_back(joint_state.name[i]);
00394     start_state.joint_state.position.push_back(joint_state.position[i]);
00395   }
00396 
00397   motion_planning_msgs::PositionConstraint position_constraint;
00398   motion_planning_msgs::OrientationConstraint orientation_constraint;
00399   motion_planning_msgs::poseStampedToPositionOrientationConstraints(end_pose, handDescription().gripperFrame(arm_name),
00400                                                                     position_constraint,
00401                                                                     orientation_constraint);
00402   motion_planning_msgs::Constraints goal_constraints;
00403   goal_constraints.position_constraints.push_back(position_constraint);
00404   goal_constraints.orientation_constraints.push_back(orientation_constraint);
00405 
00406   motion_planning_msgs::GetMotionPlan motion_plan;
00407   motion_plan.request.motion_plan_request.start_state = start_state;
00408   motion_plan.request.motion_plan_request.goal_constraints = goal_constraints;
00409   motion_plan.request.motion_plan_request.ordered_collision_operations = collision_operations;
00410 
00411   //pass the dynamic link padding, if any
00412   motion_plan.request.motion_plan_request.link_padding = link_padding;
00413 
00414   if ( !interpolated_ik_service_client_.client(arm_name).call(motion_plan) )
00415   {
00416     ROS_ERROR("  Call to Interpolated IK service failed");
00417     throw MechanismException("Call to Interpolated IK service failed");
00418   }
00419 
00420   trajectory.points.clear();
00421   trajectory.joint_names = motion_plan.response.trajectory.joint_trajectory.joint_names;
00422 
00423   if (motion_plan.response.trajectory_error_codes.empty())
00424   {
00425     ROS_ERROR("  Interpolated IK: empty trajectory received");
00426     throw MechanismException("Interpolated IK: empty trajectory received");
00427   }
00428 
00429   int error_code = motion_planning_msgs::ArmNavigationErrorCodes::SUCCESS;
00430   if (!reverse_trajectory)
00431   {
00432     for (size_t i=0; i<motion_plan.response.trajectory_error_codes.size(); i++)
00433     {
00434       if ( motion_plan.response.trajectory_error_codes[i].val == motion_plan.response.trajectory_error_codes[i].SUCCESS )
00435       {
00436         trajectory.points.push_back( motion_plan.response.trajectory.joint_trajectory.points[i] );
00437       }
00438       else
00439       {
00440         ROS_DEBUG("  Interpolated IK failed on step %d (forward towards %d) with error code %d",
00441                  (int) i,
00442                  (int) motion_plan.response.trajectory_error_codes.size() - 1,
00443                  motion_plan.response.trajectory_error_codes[i].val);
00444         error_code = motion_plan.response.trajectory_error_codes[i].val;
00445         break;
00446       }
00447     }
00448   }
00449   else
00450   {
00451     size_t first_success = 0;
00452     while ( first_success < motion_plan.response.trajectory_error_codes.size() &&
00453             motion_plan.response.trajectory_error_codes[first_success].val !=
00454             motion_plan.response.trajectory_error_codes[first_success].SUCCESS ) first_success ++;
00455     if (first_success != 0)
00456     {
00457       ROS_DEBUG("  Interpolation failed on step %d (backwards from %d) with error code %d",
00458                (int) first_success - 1,
00459                (int) motion_plan.response.trajectory_error_codes.size() - 1,
00460                motion_plan.response.trajectory_error_codes[first_success - 1].val);
00461       error_code = motion_plan.response.trajectory_error_codes[first_success - 1].val;
00462     }
00463     else
00464     {
00465       ROS_DEBUG("  Interpolation trajectory complete (backwards from %d points)",
00466                (int) motion_plan.response.trajectory_error_codes.size());
00467 
00468     }
00469     for (size_t i=first_success; i < motion_plan.response.trajectory_error_codes.size(); i++)
00470     {
00471       if ( motion_plan.response.trajectory_error_codes[i].val == motion_plan.response.trajectory_error_codes[i].SUCCESS )
00472       {
00473         trajectory.points.push_back( motion_plan.response.trajectory.joint_trajectory.points[i] );
00474       }
00475       else
00476       {
00477         ROS_ERROR("  Interpolated IK: unexpected behavior for error codes: step %d has error code %d",
00478                   (int) i, motion_plan.response.trajectory_error_codes[i].val);
00479         throw MechanismException("Interpolated IK: unexpected behavior for error codes");
00480       }
00481     }
00482   }
00483 
00484   if (!trajectory.points.empty()) actual_trajectory_length = actual_step_size * (trajectory.points.size()-1);
00485   else actual_trajectory_length = 0.0;
00486   return error_code;
00487 }
00488 
00489 bool MechanismInterface::attemptMoveArmToGoal(std::string arm_name, const std::vector<double> &desired_joint_values,
00490                                            const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
00491                                               const std::vector<motion_planning_msgs::LinkPadding> &link_padding)
00492 {
00493   //make sure joint controllers are running
00494   if(!checkController(right_joint_controller_) || !checkController(left_joint_controller_))
00495      switchToJoint();
00496 
00497   int num_tries = 0;
00498   int max_tries = 5;
00499   move_arm_msgs::MoveArmGoal move_arm_goal;
00500 
00501   move_arm_goal.motion_plan_request.group_name = handDescription().armGroup(arm_name);
00502   move_arm_goal.motion_plan_request.num_planning_attempts = 1;
00503   move_arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00504   move_arm_goal.motion_plan_request.planner_id = MOVE_ARM_PLANNER_ID;
00505   move_arm_goal.planner_service_name = MOVE_ARM_PLANNER_SERVICE_NAME;
00506   //move_arm_goal.disable_collision_monitoring = true;
00507   move_arm_goal.motion_plan_request.ordered_collision_operations = collision_operations;
00508   move_arm_goal.motion_plan_request.link_padding = link_padding;
00509 
00510   std::vector<std::string> joint_names = getJointNames(arm_name);
00511   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints.resize(joint_names.size());
00512   for(unsigned int i = 0; i < desired_joint_values.size(); i++)
00513   {
00514     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].joint_name = joint_names.at(i);
00515     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].position = desired_joint_values[i];
00516     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_below = .08;
00517     move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[i].tolerance_above = .08;
00518   }
00519 
00520   bool success = false;
00521   motion_planning_msgs::ArmNavigationErrorCodes error_code;
00522   while(num_tries < max_tries)
00523   {
00524     move_arm_action_client_.client(arm_name).sendGoal(move_arm_goal);
00525     bool withinWait = move_arm_action_client_.client(arm_name).waitForResult(ros::Duration(60.0));
00526     if(!withinWait)
00527     {
00528       move_arm_action_client_.client(arm_name).cancelGoal();
00529       ROS_DEBUG("   Move arm goal could not be achieved by move_arm in the allowed duration");
00530       success = false;
00531       num_tries++;
00532       move_arm_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00533       error_code.val = error_code.TIMED_OUT;
00534       modifyMoveArmGoal(move_arm_goal,error_code,move_arm_result.contacts);
00535       continue;
00536     }
00537     actionlib::SimpleClientGoalState state = move_arm_action_client_.client(arm_name).getState();
00538     if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
00539     {
00540       ROS_DEBUG("  Move arm: position was successfully achieved");
00541       success = true;
00542       break;
00543     }
00544     else
00545     {
00546       move_arm_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00547       ROS_DEBUG("   Move arm: non-success state was reached. Reason: %s",
00548                (motion_planning_msgs::armNavigationErrorCodeToString(move_arm_result.error_code)).c_str());
00549       ROS_DEBUG("   num_tries: %d, max_tries: %d",num_tries,max_tries);
00550       success = false;
00551       num_tries++;
00552       error_code = move_arm_result.error_code;
00553       modifyMoveArmGoal(move_arm_goal,error_code,move_arm_result.contacts);
00554       continue;
00555     }
00556   }
00558   if (!success && error_code.val == error_code.START_STATE_IN_COLLISION)
00559   {
00560     throw MoveArmStuckException();
00561   }
00562   return success;
00563 }
00564 
00565 
00566 void MechanismInterface::modifyMoveArmGoal(move_arm_msgs::MoveArmGoal &move_arm_goal,
00567                                            motion_planning_msgs::ArmNavigationErrorCodes &error_code,
00568                                            std::vector<planning_environment_msgs::ContactInformation> &contact_info_)
00569 {
00570   double allowed_penetration_depth = 0.03;
00571   if(error_code.val == error_code.START_STATE_IN_COLLISION)
00572   {
00573     std::vector<motion_planning_msgs::AllowedContactSpecification> allowed_contacts;
00574     for(unsigned int i=0; i < contact_info_.size(); i++)
00575     {
00576       if(contact_info_[i].depth < allowed_penetration_depth)
00577       {
00578         motion_planning_msgs::AllowedContactSpecification allowed_contact_tmp;
00579         allowed_contact_tmp.shape.type = allowed_contact_tmp.shape.BOX;
00580         allowed_contact_tmp.shape.dimensions.resize(3);
00581         allowed_contact_tmp.shape.dimensions[0] = 0.03;
00582         allowed_contact_tmp.shape.dimensions[1] = 0.03;
00583         allowed_contact_tmp.shape.dimensions[2] = 0.03;
00584         allowed_contact_tmp.pose_stamped.pose.position = contact_info_[i].position;
00585         allowed_contact_tmp.pose_stamped.pose.orientation.x = 0.0;
00586         allowed_contact_tmp.pose_stamped.pose.orientation.y = 0.0;
00587         allowed_contact_tmp.pose_stamped.pose.orientation.z = 0.0;
00588         allowed_contact_tmp.pose_stamped.pose.orientation.w = 1.0;
00589         allowed_contact_tmp.pose_stamped.header.stamp = ros::Time::now();
00590         allowed_contact_tmp.pose_stamped.header.frame_id = contact_info_[i].header.frame_id;
00591         allowed_contact_tmp.link_names.push_back(contact_info_[i].contact_body_1);
00592         allowed_contact_tmp.penetration_depth = allowed_penetration_depth;
00593         allowed_contacts.push_back(allowed_contact_tmp);
00594         ROS_DEBUG("Added allowed contact region: %d",i);
00595         ROS_DEBUG("Position                    : (%f,%f,%f)",contact_info_[i].position.x,
00596                  contact_info_[i].position.y,contact_info_[i].position.z);
00597         ROS_DEBUG("Frame id                    : %s",contact_info_[i].header.frame_id.c_str());
00598         ROS_DEBUG("Depth                       : %f",allowed_penetration_depth);
00599         ROS_DEBUG("Link                        : %s",contact_info_[i].contact_body_1.c_str());
00600         ROS_DEBUG(" ");
00601       }
00602     }
00603     ROS_DEBUG("Added %d allowed contact regions",(int)allowed_contacts.size());
00604     move_arm_goal.motion_plan_request.allowed_contacts = allowed_contacts;
00605   }
00606 }
00607 
00608 
00609 bool MechanismInterface::moveArmConstrained(std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose,
00610                                             const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
00611                                             const std::vector<motion_planning_msgs::LinkPadding> &link_padding,
00612                                             const motion_planning_msgs::Constraints &path_constraints,
00613                                             const double &redundancy,
00614                                             const bool &compute_viable_command_pose)
00615 {
00616 
00617   //make sure joint controllers are running
00618   if(!checkController(right_joint_controller_) || !checkController(left_joint_controller_))
00619      switchToJoint();
00620 
00621   int num_tries = 0;
00622   int max_tries = 1;
00623 
00624   move_arm_msgs::MoveArmGoal move_arm_goal;
00625   move_arm_goal.motion_plan_request.group_name = handDescription().armGroup(arm_name)+"_cartesian";
00626   move_arm_goal.motion_plan_request.num_planning_attempts = 1;
00627   move_arm_goal.motion_plan_request.allowed_planning_time = ros::Duration(5.0);
00628   move_arm_goal.motion_plan_request.planner_id = MOVE_ARM_PLANNER_ID;
00629   move_arm_goal.planner_service_name = MOVE_ARM_CONSTRAINED_PLANNER_SERVICE_NAME;
00630   move_arm_goal.motion_plan_request.ordered_collision_operations = collision_operations;
00631   move_arm_goal.motion_plan_request.link_padding = link_padding;
00632 
00633   move_arm_goal.motion_plan_request.goal_constraints.position_constraints.resize(1);
00634   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].header.frame_id =
00635     commanded_pose.header.frame_id;
00636   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].link_name =
00637     handDescription().gripperFrame(arm_name);
00638   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position = commanded_pose.pose.position;
00639   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_shape.type =
00640     geometric_shapes_msgs::Shape::BOX;
00641   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].
00642     constraint_region_shape.dimensions.push_back(OBJECT_POSITION_TOLERANCE_X);
00643   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].
00644     constraint_region_shape.dimensions.push_back(OBJECT_POSITION_TOLERANCE_Y);
00645   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].
00646     constraint_region_shape.dimensions.push_back(OBJECT_POSITION_TOLERANCE_Z);
00647   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].constraint_region_orientation.w = 1.0;
00648   move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].weight = 1.0;
00649 
00650   move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints.resize(1);
00651   move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0] =
00652     path_constraints.orientation_constraints[0];
00653   move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0].header.stamp = ros::Time::now();
00654 
00655   btQuaternion orientation;
00656   tf::quaternionMsgToTF(commanded_pose.pose.orientation,orientation);
00657   geometry_msgs::PoseStamped desired_pose = commanded_pose;
00658   desired_pose.pose.position = move_arm_goal.motion_plan_request.goal_constraints.position_constraints[0].position;
00659   desired_pose.header.stamp = ros::Time::now();
00660 
00661   // TODO - this should really happen through the constraint
00662   // Currently its hard-coded for keeping objects level
00663   if(compute_viable_command_pose && path_constraints.orientation_constraints.size())
00664   {
00665     bool get_ik = false;
00666     for(unsigned int i=0; i < 360; i++)
00667     {
00668       double rotation_yaw = i*M_PI/360.0;
00669       ROS_DEBUG("Trying to find suitable goal orientation with yaw rotation: %f",rotation_yaw);
00670       btQuaternion orientation_yaw;
00671       orientation_yaw.setRPY(0.0,0.0,rotation_yaw);
00672       orientation_yaw *= orientation;
00673       geometry_msgs::Quaternion quaternion;
00674       tf::quaternionTFToMsg(orientation_yaw,quaternion);
00675       desired_pose.pose.orientation = quaternion;
00676       desired_pose.header.stamp = ros::Time::now();
00677       kinematics_msgs::GetConstraintAwarePositionIK::Response ik_response;
00678       if(getIKForPose(arm_name,desired_pose,ik_response, collision_operations, link_padding))
00679       {
00680         get_ik = true;
00681         break;
00682       }
00683     }
00684     if(!get_ik)
00685       return false;
00686   }
00687 
00688   move_arm_goal.motion_plan_request.path_constraints.orientation_constraints.resize(1);
00689   move_arm_goal.motion_plan_request.path_constraints.orientation_constraints[0] =
00690     move_arm_goal.motion_plan_request.goal_constraints.orientation_constraints[0];
00691   move_arm_goal.disable_ik = true;
00692 
00693   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints.resize(1);
00694   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].joint_name = (getJointNames(arm_name))[2];
00695   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].position = redundancy;
00696   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].tolerance_below = M_PI;
00697   move_arm_goal.motion_plan_request.goal_constraints.joint_constraints[0].tolerance_above = M_PI;
00698 
00699   bool success = false;
00700   while(num_tries < max_tries)
00701   {
00702     move_arm_action_client_.client(arm_name).sendGoal(move_arm_goal);
00703     bool withinWait = move_arm_action_client_.client(arm_name).waitForResult(ros::Duration(60.0));
00704     if(!withinWait)
00705     {
00706       move_arm_action_client_.client(arm_name).cancelGoal();
00707       ROS_DEBUG("  Move arm goal could not be achieved by move_arm in the allowed duration");
00708       success = false;
00709       num_tries++;
00710       move_arm_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00711       motion_planning_msgs::ArmNavigationErrorCodes error_code;
00712       error_code.val = error_code.TIMED_OUT;
00713       modifyMoveArmGoal(move_arm_goal,error_code,move_arm_result.contacts);
00714       continue;
00715     }
00716     actionlib::SimpleClientGoalState state = move_arm_action_client_.client(arm_name).getState();
00717     if(state == actionlib::SimpleClientGoalState::SUCCEEDED)
00718     {
00719       ROS_DEBUG("  Move arm: position was successfully achieved");
00720       success = true;
00721       break;
00722     }
00723     else
00724     {
00725       move_arm_msgs::MoveArmResult move_arm_result = *move_arm_action_client_.client(arm_name).getResult();
00726       ROS_DEBUG("Move arm: non-success state was reached. Reason: %s",
00727                (motion_planning_msgs::armNavigationErrorCodeToString(move_arm_result.error_code)).c_str());
00728       ROS_DEBUG("num_tries: %d, max_tries: %d",num_tries,max_tries);
00729       success = false;
00730       num_tries++;
00731       modifyMoveArmGoal(move_arm_goal,move_arm_result.error_code,move_arm_result.contacts);
00732       continue;
00733     }
00734   }
00735   return success;
00736 }
00737 
00749 geometry_msgs::PoseStamped MechanismInterface::translateGripperPose(geometry_msgs::Vector3Stamped translation,
00750                                                                     geometry_msgs::PoseStamped start_pose,
00751                                                                     std::string arm_name)
00752 {
00753 
00754    ROS_INFO("translate Gripper Pose 1");
00755   bool pre_multiply;
00756   if (translation.header.frame_id == handDescription().gripperFrame(arm_name))
00757   {
00758     pre_multiply=false;
00759   }
00760   else if (translation.header.frame_id == handDescription().robotFrame(arm_name))
00761   {
00762     pre_multiply=true;
00763   }
00764   else
00765   {
00766     throw MechanismException(std::string("Gripper translations (such as for lift or approach) can only be specified in "
00767                                      "either the gripper frame or the robot frame. This one was specified in frame ") +
00768                              translation.header.frame_id);
00769   }
00770 
00771 
00772   //go to robot frame first
00773   geometry_msgs::PoseStamped start_pose_robot_frame = transformPose(handDescription().robotFrame(arm_name), start_pose);
00774   tf::StampedTransform start_transform;
00775   tf::poseMsgToTF(start_pose_robot_frame.pose, start_transform);
00776 
00777   btVector3 vec;
00778   tf::vector3MsgToTF(translation.vector, vec);
00779   tf::StampedTransform translate_trans;
00780   translate_trans.setIdentity();
00781   translate_trans.setOrigin(vec);
00782   //compute the translated pose
00783   tf::Transform end_transform;
00784   if (pre_multiply) end_transform = translate_trans * start_transform;
00785   else end_transform = start_transform * translate_trans;
00786   //prepare the results
00787   geometry_msgs::PoseStamped translated_pose;
00788   tf::poseTFToMsg(end_transform, translated_pose.pose);
00789   translated_pose.header.frame_id = handDescription().robotFrame(arm_name);
00790   translated_pose.header.stamp = ros::Time(0);
00791 
00792   //return the result in the requested frame
00793   translated_pose = transformPose(start_pose.header.frame_id, translated_pose);
00794   return translated_pose;
00795 }
00796 
00798 geometry_msgs::PoseStamped MechanismInterface::getGripperPose(std::string arm_name, std::string frame_id)
00799 {
00800   tf::StampedTransform gripper_transform;
00801   try
00802   {
00803     listener_.lookupTransform(frame_id, handDescription().gripperFrame(arm_name), ros::Time(0), gripper_transform);
00804   }
00805   catch (tf::TransformException ex)
00806   {
00807     ROS_ERROR("Mechanism interface: failed to get tf transform for wrist roll link; tf exception %s", ex.what());
00808     throw MechanismException(std::string("failed to get tf transform for wrist roll link; tf exception: ") +
00809                              std::string(ex.what()) );
00810   }
00811   geometry_msgs::PoseStamped gripper_pose;
00812   tf::poseTFToMsg(gripper_transform, gripper_pose.pose);
00813   gripper_pose.header.frame_id = frame_id;
00814   gripper_pose.header.stamp = ros::Time::now();
00815   return gripper_pose;
00816 }
00817 
00818 void MechanismInterface::transformPointCloud(std::string target_frame,
00819                                              const sensor_msgs::PointCloud &cloud_in,
00820                                              sensor_msgs::PointCloud &cloud_out)
00821 {
00822   try
00823   {
00824     listener_.transformPointCloud(target_frame, cloud_in, cloud_out);
00825   }
00826   catch (tf::TransformException ex)
00827   {
00828     ROS_ERROR("Mechanism interface: failed to cloud into %s frame; exception: %s", target_frame.c_str(),
00829               ex.what());
00830     throw MechanismException(std::string("failed to transform cloud into frame ") + target_frame +
00831                              std::string("; tf exception: ") + std::string(ex.what()) );
00832   }
00833 }
00834 
00835 void MechanismInterface::convertGraspableObjectComponentsToFrame(object_manipulation_msgs::GraspableObject &object,
00836                                                                  std::string frame_id)
00837 {
00838   if (!object.cluster.points.empty())
00839   {
00840     transformPointCloud(frame_id, object.cluster, object.cluster);
00841   }
00842   for (size_t i=0; i<object.potential_models.size(); i++)
00843   {
00844     object.potential_models[i].pose = transformPose(frame_id, object.potential_models[i].pose);
00845   }
00846   object.reference_frame_id = frame_id;
00847 }
00848 
00849 geometry_msgs::PoseStamped MechanismInterface::transformPose(const std::string target_frame,
00850                                                              const geometry_msgs::PoseStamped &stamped_in)
00851 {
00852   geometry_msgs::PoseStamped stamped_out;
00853   try
00854   {
00855     listener_.transformPose(target_frame, stamped_in, stamped_out);
00856   }
00857   catch (tf::TransformException ex)
00858   {
00859     ROS_ERROR("Mechanism interface: failed to transform pose into %s frame; exception: %s", target_frame.c_str(),
00860               ex.what());
00861     throw MechanismException(std::string("failed to transform pose into frame ") + target_frame +
00862                              std::string("; tf exception: ") + std::string(ex.what()) );
00863   }
00864   return stamped_out;
00865 }
00866 
00869 bool MechanismInterface::translateGripper(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
00870                                           motion_planning_msgs::OrderedCollisionOperations ord,
00871                                           const std::vector<motion_planning_msgs::LinkPadding> &link_padding,
00872                                           float requested_distance, float min_distance,
00873                                           float &actual_distance)
00874 {
00875   //get the current gripper pose in the robot frame
00876   geometry_msgs::PoseStamped start_pose_stamped = getGripperPose(arm_name, handDescription().robotFrame(arm_name));
00877 
00878   //compute the interpolated trajectory
00879   trajectory_msgs::JointTrajectory traj;
00880   std::vector<double> empty;
00881   sensor_msgs::JointState empty_state;
00882   getInterpolatedIK(arm_name,
00883                     start_pose_stamped, direction, requested_distance,
00884                     empty, empty_state, ord, link_padding, false, traj, actual_distance);
00885 
00886   if (min_distance > 0 && actual_distance < min_distance)
00887   {
00888     ROS_DEBUG("Mechanism interface: interpolated IK trajectory covers %f distance, but at least %f requested",
00889              actual_distance, min_distance);
00890     actual_distance = 0;
00891     return false;
00892   }
00893 
00894   if (traj.points.empty())
00895   {
00896     ROS_DEBUG("Mechanism interface: translate gripper trajectory is empty");
00897     actual_distance = false;
00898     return false;
00899   }
00900 
00901   //execute the normalized interpolated trajectory
00902   attemptTrajectory(arm_name, traj, true);
00903   return true;
00904 }
00905 
00909 geometry_msgs::PoseStamped MechanismInterface::getObjectPoseForGrasp(std::string arm_name,
00910                                                                      const geometry_msgs::Pose &grasp_pose)
00911 
00912 {
00913   //get the current pose of the gripper in base link coordinate frame
00914   tf::StampedTransform wrist_transform;
00915   try
00916   {
00917     listener_.lookupTransform("base_link",handDescription().gripperFrame(arm_name),
00918                               ros::Time(0), wrist_transform);
00919   }
00920   catch (tf::TransformException ex)
00921   {
00922     ROS_ERROR("Mechanism interface: failed to get tf transform for wrist roll link");
00923     throw MechanismException(std::string("failed to get tf transform for wrist roll link; tf exception: ") +
00924                              std::string(ex.what()) );
00925   }
00926 
00927   //multiply by inverse of grasp pose
00928   tf::Transform grasp_transform;
00929   tf::poseMsgToTF(grasp_pose, grasp_transform);
00930 
00931   tf::Transform object_transform;
00932   object_transform = wrist_transform * grasp_transform.inverse();
00933 
00934   //prepare the result
00935   geometry_msgs::PoseStamped object_pose;
00936   tf::poseTFToMsg(object_transform, object_pose.pose);
00937   object_pose.header.frame_id = "base_link";
00938   object_pose.header.stamp = ros::Time::now();
00939   return object_pose;
00940 }
00941 
00942 void MechanismInterface::attachObjectToGripper(std::string arm_name, std::string collision_object_name)
00943 {
00944   mapping_msgs::AttachedCollisionObject obj;
00945   obj.object.header.stamp = ros::Time::now();
00946   obj.object.header.frame_id = handDescription().robotFrame(arm_name);
00947   obj.object.operation.operation = mapping_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT;
00948   obj.object.id = collision_object_name;
00949   obj.link_name = handDescription().attachLinkName(arm_name);
00950   obj.touch_links = handDescription().gripperTouchLinkNames(arm_name);
00951   attached_object_pub_.publish(obj);
00952 }
00953 
00954 void MechanismInterface::detachAndAddBackObjectsAttachedToGripper(std::string arm_name,
00955                                                                   std::string collision_object_name)
00956 {
00957   mapping_msgs::AttachedCollisionObject att;
00958   att.object.header.stamp = ros::Time::now();
00959   att.object.header.frame_id = handDescription().robotFrame(arm_name);
00960   att.link_name = handDescription().attachLinkName(arm_name);
00961   att.object.id = collision_object_name;
00962   att.object.operation.operation = mapping_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT;
00963   attached_object_pub_.publish(att);
00964 }
00965 
00966 void MechanismInterface::handPostureGraspAction(std::string arm_name,
00967                                                 const object_manipulation_msgs::Grasp &grasp, int goal)
00968 {
00969   object_manipulation_msgs::GraspHandPostureExecutionGoal posture_goal;
00970   posture_goal.grasp = grasp;
00971   posture_goal.goal = goal;
00972   hand_posture_client_.client(arm_name).sendGoal(posture_goal);
00973   bool withinWait = hand_posture_client_.client(arm_name).waitForResult(ros::Duration(10.0));
00974   if(!withinWait)
00975   {
00976     hand_posture_client_.client(arm_name).cancelGoal();
00977     ROS_ERROR("Hand posture controller timed out on goal (%d)", goal);
00978     throw MechanismException("Hand posture controller timed out");
00979   }
00980   actionlib::SimpleClientGoalState state = hand_posture_client_.client(arm_name).getState();
00981   if(state != actionlib::SimpleClientGoalState::SUCCEEDED)
00982   {
00983     ROS_ERROR("Hand posture controller failed on goal (%d)", goal);
00984     throw MechanismException("Hand posture controller failed");
00985   }
00986   ROS_DEBUG("Hand posture controller successfully achieved goal %d", goal);
00987 }
00988 
00989 bool MechanismInterface::graspPostureQuery(std::string arm_name)
00990 {
00991   object_manipulation_msgs::GraspStatus query;
00992   if (!grasp_status_client_.client(arm_name).call(query))
00993   {
00994     ROS_ERROR("Grasp posture query call failed");
00995     throw MechanismException("Grasp posture query call failed");
00996   }
00997   return query.response.is_hand_occupied;
00998 }
00999 
01000 bool MechanismInterface::pointHeadAction(const geometry_msgs::PointStamped &target, std::string pointing_frame)
01001 {
01002   pr2_controllers_msgs::PointHeadGoal goal;
01003   goal.target = target;
01004   goal.pointing_axis.x = 0;
01005   goal.pointing_axis.y = 0;
01006   goal.pointing_axis.z = 1;
01007   goal.pointing_frame = pointing_frame;
01008   goal.min_duration = ros::Duration(1.0);
01009   goal.max_velocity = 1.0;
01010 
01011   point_head_action_client_.client().sendGoal(goal);
01012   point_head_action_client_.client().waitForResult( ros::Duration(3.0) );
01013 
01014   if (point_head_action_client_.client().getState() == actionlib::SimpleClientGoalState::SUCCEEDED)
01015   {
01016     ROS_DEBUG("Successfully moved head.");
01017     return true;
01018   }
01019   else
01020   {
01021     ROS_ERROR("Head movement failed or timed out.");
01022     return false;
01023   }
01024 
01025 }
01026 
01027 bool MechanismInterface::callSwitchControllers(std::vector<std::string> start_controllers,
01028                                                std::vector<std::string> stop_controllers)
01029 {
01030   pr2_mechanism_msgs::SwitchController srv;
01031   srv.request.start_controllers = start_controllers;
01032   srv.request.stop_controllers = stop_controllers;
01033   srv.request.strictness = srv.request.STRICT;
01034   if ( !switch_controller_service_.client().call(srv) )
01035   {
01036     ROS_ERROR("Mechanism interface: switch controller service call failed");
01037     throw MechanismException("switch controller service call failed");
01038   }
01039   if(!srv.response.ok) return false;
01040   return true;
01041 }
01042 
01043 bool MechanismInterface::switchControllers(std::string start_controller, std::string stop_controller)
01044 {
01045   ROS_DEBUG("Switching controller %s for %s", start_controller.c_str(), stop_controller.c_str());
01046   std::vector<std::string> start_controllers;
01047   std::vector<std::string> stop_controllers;
01048   start_controllers.push_back(start_controller);
01049   stop_controllers.push_back(stop_controller);
01050   bool success = callSwitchControllers(start_controllers, stop_controllers);
01051   if(success)
01052   {
01053     bool start_running = checkController(start_controller);
01054     bool stop_running = checkController(stop_controller);
01055     if(start_running && !stop_running) return true;
01056     ROS_ERROR("switching %s to %s failed even though it returned success",
01057               stop_controller.c_str(), start_controller.c_str());
01058     return false;
01059   }
01060   else
01061   {
01062     ROS_ERROR("switching %s to %s failed", stop_controller.c_str(), start_controller.c_str());
01063     return false;
01064   }
01065 }
01066 
01067 bool MechanismInterface::checkController(std::string controller)
01068 {
01069   return true;
01070 }
01071 
01072 bool MechanismInterface::startController(std::string controller)
01073 {
01074   ROS_DEBUG("Starting controller %s", controller.c_str());
01075   std::vector<std::string> start_controllers;
01076   std::vector<std::string> stop_controllers;
01077   start_controllers.push_back(controller);
01078   bool success = callSwitchControllers(start_controllers, stop_controllers);
01079   if(success)
01080   {
01081     bool running = checkController(controller);
01082     if(running) return true;
01083     ROS_ERROR("starting controller %s failed even though it returned success", controller.c_str());
01084     return false;
01085   }
01086   else
01087   {
01088     ROS_ERROR("starting controller %s failed", controller.c_str());
01089     return false;
01090   }
01091 }
01092 
01093 bool MechanismInterface::stopController(std::string controller)
01094 {
01095   ROS_DEBUG("Stopping controller %s", controller.c_str());
01096   std::vector<std::string> start_controllers;
01097   std::vector<std::string> stop_controllers;
01098   stop_controllers.push_back(controller);
01099   bool success = callSwitchControllers(start_controllers, stop_controllers);
01100   if(success)
01101   {
01102     bool running = checkController(controller);
01103     if(running)
01104     {
01105       ROS_ERROR("stopping controller %s failed even though it returned success", controller.c_str());
01106       return false;
01107     }
01108     return true;
01109   }
01110   else
01111   {
01112     ROS_ERROR("stopping controller %s failed", controller.c_str());
01113     return false;
01114   }
01115 }
01116 
01117 bool MechanismInterface::switchToCartesian()
01118 {
01119   bool result = switchControllers(right_cartesian_controller_, right_joint_controller_);
01120   if(!result) return false;
01121   result = switchControllers(left_cartesian_controller_, left_joint_controller_);
01122   if(!result) return false;
01123   return true;
01124 }
01125 
01126 bool MechanismInterface::switchToJoint()
01127 {
01128   bool result = switchControllers(right_joint_controller_, right_cartesian_controller_);
01129   if(!result) return false;
01130   result = switchControllers(left_joint_controller_, left_cartesian_controller_);
01131   if(!result) return false;
01132   return true;
01133 }
01134 
01135 geometry_msgs::PoseStamped MechanismInterface::clipDesiredPose(std::string arm_name,
01136                                                                const geometry_msgs::PoseStamped &desired_pose,
01137                                                                double clip_dist, double clip_angle)
01138 {
01139   //no clipping desired
01140   if(clip_dist == 0 && clip_angle == 0) return desired_pose;
01141 
01142   //Get the current gripper pose
01143   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01144 
01145   //Get the position and angle dists between current and desired
01146   Eigen::eigen2_Transform3d current_trans, desired_trans;
01147   double pos_dist, angle;
01148   Eigen::Vector3d axis, direction;
01149   tf::poseMsgToEigen(current_pose.pose, current_trans);
01150   tf::poseMsgToEigen(desired_pose.pose, desired_trans);
01151   positionAndAngleDist(current_trans, desired_trans, pos_dist, angle, axis, direction);
01152 
01153   //Clip the desired pose to be at most clip_dist and the desired angle to be at most clip_angle (proportional)
01154   //from the current
01155   double pos_fraction, angle_fraction;
01156   double pos_change, angle_change;
01157   pos_fraction = fabs(angle / clip_angle);
01158   angle_fraction = fabs(pos_dist / clip_dist);
01159   if(pos_fraction <=1 && angle_fraction <=1){
01160     return desired_pose;
01161   }
01162   double fraction = pos_fraction;
01163   if(angle_fraction > pos_fraction) fraction = angle_fraction;
01164   pos_change = pos_dist / fraction;
01165   angle_change = angle / fraction;
01166 
01167   Eigen::eigen2_Transform3d clipped_trans;
01168   clipped_trans = current_trans;
01169   Eigen::Vector3d scaled_direction;
01170   scaled_direction = direction * pos_change;
01171   Eigen::eigen2_Translation3d translation(scaled_direction);
01172   clipped_trans = clipped_trans * translation;
01173   Eigen::eigen2_AngleAxis<double> angle_axis(angle_change, axis);
01174   clipped_trans = clipped_trans * angle_axis;
01175   geometry_msgs::PoseStamped clipped_pose;
01176   tf::poseEigenToMsg(clipped_trans, clipped_pose.pose);
01177   clipped_pose.header = desired_pose.header;
01178   return clipped_pose;
01179 }
01180 
01181 void MechanismInterface::poseDists(geometry_msgs::Pose start, geometry_msgs::Pose end, double &pos_dist, double &angle)
01182 {
01183   Eigen::eigen2_Transform3d start_trans, end_trans;
01184   tf::poseMsgToEigen(start, start_trans);
01185   tf::poseMsgToEigen(end, end_trans);
01186   Eigen::Vector3d axis, direction;
01187   positionAndAngleDist(start_trans, end_trans, pos_dist, angle, axis, direction);
01188 }
01189 
01190 void MechanismInterface::positionAndAngleDist(Eigen::eigen2_Transform3d start, Eigen::eigen2_Transform3d end,
01191                                               double &pos_dist,
01192                                               double &angle, Eigen::Vector3d &axis, Eigen::Vector3d &direction)
01193 {
01194   //trans = end to start = global to start * end to global
01195   Eigen::eigen2_Transform3d trans;
01196   trans = start.inverse() * end;
01197   Eigen::eigen2_AngleAxis<double> angle_axis;
01198   angle_axis = trans.rotation();
01199   angle = angle_axis.angle();
01200   axis = angle_axis.axis();
01201   direction = trans.translation();
01202   pos_dist = sqrt(direction.dot(direction));
01203   if(pos_dist) direction *= 1/pos_dist;
01204 }
01205 
01206 int MechanismInterface::translateGripperCartesian(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
01207                                                   ros::Duration timeout, double dist_tol = .01, double angle_tol = .09,
01208                                                 double clip_dist = .02, double clip_angle = .16, double timestep = 0.1)
01209 {
01210   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, direction.header.frame_id);
01211   geometry_msgs::PoseStamped desired_pose = translateGripperPose(direction, current_pose, arm_name);
01212   int result = moveArmToPoseCartesian(arm_name, desired_pose, timeout, dist_tol, angle_tol,
01213                                       clip_dist, clip_angle, timestep);
01214   return result;
01215 }
01216 
01217 //returns 0 if an error occurred, 1 if it got there, -1 if it timed out
01218 int MechanismInterface::moveArmToPoseCartesian(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
01219                                                ros::Duration timeout, double dist_tol = .015, double angle_tol = .09,
01220                                                double clip_dist = .02, double clip_angle = .16, double timestep = 0.1)
01221 {
01222   bool success = false;
01223 
01224   //Switch to Cartesian controllers
01225   for(int tries=0; tries<3; tries++)
01226   {
01227     success = switchToCartesian();
01228     if(success) break;
01229     ros::Duration(1.0).sleep();
01230   }
01231   if(!success)
01232   {
01233     ROS_ERROR("Tries exceeded when trying to switch to Cartesian control!");
01234     return 0;
01235   }
01236 
01237   //Move towards the desired pose until we get there within our tolerance or until time runs out
01238   ros::Time begin = ros::Time::now();
01239   int reached_target = -1;
01240   geometry_msgs::PoseStamped current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01241 
01242   while(1)
01243   {
01244     //stop if we're out of time
01245     if(ros::Time::now() - begin > timeout)
01246     {
01247       ROS_DEBUG("Timed out when moving to desired Cartesian pose");
01248       break;
01249     }
01250 
01251     //stop if we're within our tolerances
01252     current_pose = getGripperPose(arm_name, desired_pose.header.frame_id);
01253     double pos_dist, angle_dist;
01254     poseDists(current_pose.pose, desired_pose.pose, pos_dist, angle_dist);
01255     if(pos_dist <= dist_tol && angle_dist <= angle_tol)
01256     {
01257       reached_target = 1;
01258       break;
01259     }
01260 
01261     //clip the desired pose and send it out
01262     sendCartesianPoseCommand(arm_name, desired_pose, clip_dist, clip_angle);
01263 
01264     //sleep until the next timestep
01265     ros::Duration(timestep).sleep();
01266   }
01267 
01268   //Switch back to joint control
01269   success = false;
01270   for(int tries=0; tries<3; tries++)
01271   {
01272     success = switchToJoint();
01273     if(success) break;
01274     ros::Duration(1.0).sleep();
01275   }
01276   if(!success)
01277   {
01278     ROS_ERROR("Tries exceeding when trying to switch back to joint control!");
01279     return 0;
01280   }
01281   return reached_target;
01282 }
01283 
01284 void MechanismInterface::sendCartesianPoseCommand(std::string arm_name, geometry_msgs::PoseStamped desired_pose,
01285                                                   double clip_dist = 0, double clip_angle = 0)
01286 {
01287   //geometry_msgs::PoseStamped clipped_pose = desired_pose;
01288   geometry_msgs::PoseStamped clipped_pose = clipDesiredPose(arm_name, desired_pose, clip_dist, clip_angle);
01289   if(arm_name.compare("right_arm") == 0)
01290   {
01291     right_cartesian_pub_.publish(clipped_pose);
01292   }
01293   else if(arm_name.compare("left_arm") == 0)
01294   {
01295     left_cartesian_pub_.publish(clipped_pose);
01296   }
01297   else
01298   {
01299     ROS_ERROR("arm_name '%s' not recognized", arm_name.c_str());
01300   }
01301 }
01302 
01303 std::vector<motion_planning_msgs::LinkPadding>
01304 MechanismInterface::fingertipPadding(std::string arm_name, double pad)
01305 {
01306   std::vector<motion_planning_msgs::LinkPadding> padding_vec;
01307   motion_planning_msgs::LinkPadding padding;
01308   padding.padding = pad;
01309   std::vector<std::string> links = handDescription().fingertipLinks(arm_name);
01310   for (size_t i=0; i<links.size(); i++)
01311   {
01312     padding.link_name = links[i];
01313     padding_vec.push_back(padding);
01314   }
01315   return padding_vec;
01316 }
01317 
01318 std::vector<motion_planning_msgs::LinkPadding>
01319 MechanismInterface::gripperPadding(std::string arm_name, double pad)
01320 {
01321   std::vector<motion_planning_msgs::LinkPadding> padding_vec;
01322   motion_planning_msgs::LinkPadding padding;
01323   padding.padding = pad;
01324   std::vector<std::string> links = handDescription().gripperTouchLinkNames(arm_name);
01325   for (size_t i=0; i<links.size(); i++)
01326   {
01327     padding.link_name = links[i];
01328     padding_vec.push_back(padding);
01329   }
01330   return padding_vec;
01331 }
01332 
01333 } //namespace object_manipulator
mechanism_interface.cpp
