mechanism_interface.h
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 #ifndef _MECHANISM_INTERFACE_H_
00034 #define _MECHANISM_INTERFACE_H_
00035 
00036 #include <ros/ros.h>
00037 
00038 #include <actionlib/client/simple_action_client.h>
00039 
00040 #include <tf/transform_listener.h>
00041 
00042 #include <eigen_conversions/eigen_msg.h>
00043 #include <Eigen/Core>
00044 #include <Eigen/Geometry>
00045 
00046 #include <geometry_msgs/Vector3.h>
00047 
00048 #include <kinematics_msgs/GetKinematicSolverInfo.h>
00049 #include <kinematics_msgs/GetConstraintAwarePositionIK.h>
00050 #include <kinematics_msgs/GetPositionFK.h>
00051 
00052 #include <arm_navigation_msgs/MoveArmAction.h>
00053 
00054 #include <arm_navigation_msgs/GetMotionPlan.h>
00055 #include <arm_navigation_msgs/convert_messages.h>
00056 #include <arm_navigation_msgs/FilterJointTrajectory.h>
00057 #include <arm_navigation_msgs/OrderedCollisionOperations.h>
00058 
00059 #include <pr2_controllers_msgs/JointTrajectoryAction.h>
00060 #include <pr2_controllers_msgs/PointHeadAction.h>
00061 
00062 #include <arm_navigation_msgs/ContactInformation.h>
00063 #include <arm_navigation_msgs/GetStateValidity.h>
00064 #include <arm_navigation_msgs/SetPlanningSceneDiff.h>
00065 #include <arm_navigation_msgs/GetRobotState.h>
00066 
00067 #include <std_srvs/Empty.h>
00068 
00069 #include <interpolated_ik_motion_planner/SetInterpolatedIKMotionPlanParams.h>
00070 
00071 #include <arm_navigation_msgs/AttachedCollisionObject.h>
00072 
00073 #include <planning_environment/models/collision_models.h>
00074 
00075 #include <object_manipulation_msgs/ReactiveGraspAction.h>
00076 #include <object_manipulation_msgs/ReactiveLiftAction.h>
00077 #include <object_manipulation_msgs/ReactivePlaceAction.h>
00078 #include <object_manipulation_msgs/GraspHandPostureExecutionAction.h>
00079 #include <object_manipulation_msgs/GraspStatus.h>
00080 
00081 #include <pr2_mechanism_msgs/SwitchController.h>
00082 #include <pr2_mechanism_msgs/ListControllers.h>
00083 
00084 #include "object_manipulator/tools/exceptions.h"
00085 #include "object_manipulator/tools/service_action_wrappers.h"
00086 
00087 
00088 namespace object_manipulator {
00089 
00091 class MechanismInterface
00092 {
00093  private:
00095   ros::NodeHandle root_nh_;
00096 
00098   ros::NodeHandle priv_nh_;
00099 
00101   tf::TransformListener listener_;
00102 
00104   ros::Publisher attached_object_pub_;
00105 
00107   std::string joint_states_topic_;
00108 
00111   std::map<std::string, std::string> joint_controller_names_; 
00112 
00115   std::map<std::string, std::string> cartesian_controller_names_; 
00116 
00118   arm_navigation_msgs::OrderedCollisionOperations collision_operations_cache_;
00119 
00121   std::vector<arm_navigation_msgs::LinkPadding> link_padding_cache_;
00122 
00123   planning_environment::CollisionModels cm_;
00124   planning_models::KinematicState* planning_scene_state_;
00125 
00127   bool planning_scene_cache_empty_;
00128 
00130   bool cache_planning_scene_;
00131 
00133   void setInterpolatedIKParams(std::string arm_name, int num_steps, 
00134                                int collision_check_resolution, bool start_from_end);
00135 
00137   bool callSwitchControllers(std::vector<std::string> start_controllers, std::vector<std::string> stop_controllers);
00138   
00140 
00141   bool cachePlanningScene(const arm_navigation_msgs::OrderedCollisionOperations& collision_operations,
00142                           const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);
00143 
00144  public:
00145 
00146   //----------------------------- Service clients -------------------------------
00147 
00149   MultiArmServiceWrapper<kinematics_msgs::GetKinematicSolverInfo> ik_query_client_;
00150 
00152   MultiArmServiceWrapper<kinematics_msgs::GetConstraintAwarePositionIK> ik_service_client_;
00153 
00155   MultiArmServiceWrapper<kinematics_msgs::GetPositionFK> fk_service_client_;
00156 
00158   MultiArmServiceWrapper<arm_navigation_msgs::GetMotionPlan> interpolated_ik_service_client_;  
00159 
00161   MultiArmServiceWrapper<interpolated_ik_motion_planner::SetInterpolatedIKMotionPlanParams> 
00162     interpolated_ik_set_params_client_;
00163 
00165   MultiArmServiceWrapper<object_manipulation_msgs::GraspStatus> grasp_status_client_;
00166 
00168   ServiceWrapper<arm_navigation_msgs::GetStateValidity> check_state_validity_client_;
00169 
00171   ServiceWrapper<arm_navigation_msgs::FilterJointTrajectory> joint_trajectory_normalizer_service_;
00172   
00174   ServiceWrapper<pr2_mechanism_msgs::SwitchController> switch_controller_service_;
00175 
00177   ServiceWrapper<pr2_mechanism_msgs::ListControllers> list_controllers_service_;
00178 
00180   ServiceWrapper<arm_navigation_msgs::GetRobotState> get_robot_state_client_;
00181 
00183   ServiceWrapper<arm_navigation_msgs::SetPlanningSceneDiff> set_planning_scene_diff_service_;
00184 
00186   ServiceWrapper<std_srvs::Empty> reset_collision_map_service_;
00187 
00188   //------------------------------ Action clients -------------------------------
00189 
00191   MultiArmActionWrapper<object_manipulation_msgs::ReactiveGraspAction> reactive_grasp_action_client_;
00192 
00194   MultiArmActionWrapper<object_manipulation_msgs::ReactiveLiftAction> reactive_lift_action_client_;
00195 
00197   MultiArmActionWrapper<object_manipulation_msgs::ReactivePlaceAction> reactive_place_action_client_;
00198 
00200   MultiArmActionWrapper<arm_navigation_msgs::MoveArmAction> move_arm_action_client_;  
00201 
00203   MultiArmActionWrapper<pr2_controllers_msgs::JointTrajectoryAction> traj_action_client_;
00204 
00206   MultiArmActionWrapper<object_manipulation_msgs::GraspHandPostureExecutionAction> hand_posture_client_;
00207 
00209   ActionWrapper<pr2_controllers_msgs::PointHeadAction> point_head_action_client_;
00210 
00211   //------------------------------ Publishers ----------------------------------
00212 
00214   MultiArmTopicWrapper<geometry_msgs::PoseStamped> cartesian_pub_;
00215 
00217   MultiArmTopicWrapper<std_msgs::Float64MultiArray> cartesian_posture_pub_;
00218 
00220   //MultiArmTopicWrapper<std_msgs::Float64MultiArray> cartesian_gains_pub_;
00221   //MultiArmTopicWrapper<std_msgs::CartesianGains> cartesian_gains_pub_;
00222 
00223   //------------------------------ Functionality -------------------------------
00224 
00226   MechanismInterface();
00227 
00228   ~MechanismInterface() {
00229     if(planning_scene_state_ != NULL) {
00230       cm_.revertPlanningScene(planning_scene_state_);
00231     }
00232   }
00233 
00234   planning_environment::CollisionModels& getCollisionModels() {
00235     return cm_;
00236   }
00237   
00238   planning_models::KinematicState* getPlanningSceneState() const {
00239     return planning_scene_state_;
00240   }
00241 
00242   //------------- IK -------------
00243 
00245   void getRobotState(arm_navigation_msgs::RobotState& state);
00246 
00249   void getPlanningScene(const arm_navigation_msgs::OrderedCollisionOperations& collision_operations,
00250                         const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);
00251 
00253   bool checkStateValidity(std::string arm_name, const std::vector<double> &joint_values,
00254                           const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00255                           const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);
00256 
00258   bool checkStateValidity(std::string arm_name, const std::vector<double> &joint_values)
00259   {
00260     arm_navigation_msgs::OrderedCollisionOperations empty;
00261     std::vector<arm_navigation_msgs::LinkPadding> also_empty;
00262     return checkStateValidity(arm_name, joint_values, empty, also_empty);
00263   }
00264 
00266   bool getIKForPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00267                     kinematics_msgs::GetConstraintAwarePositionIK::Response& ik_response,
00268                     const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00269                     const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);
00270 
00272   bool getFK(std::string arm_name, 
00273              std::vector<double> positions, 
00274              geometry_msgs::PoseStamped &pose_stamped);
00275 
00276 
00278   int getInterpolatedIK(std::string arm_name,
00279                         geometry_msgs::PoseStamped start_pose,
00280                         geometry_msgs::Vector3Stamped direction,
00281                         float desired_trajectory_length,
00282                         const std::vector<double> &seed_joint_position,
00283                         const sensor_msgs::JointState &joint_state,
00284                         const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00285                         const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00286                         bool reverse_trajectory,
00287                         trajectory_msgs::JointTrajectory &trajectory,
00288                         float &actual_trajectory_length);
00289 
00290   //------ traj controller  ------
00291 
00293   void attemptTrajectory(std::string arm_name, const trajectory_msgs::JointTrajectory &trajectory, bool unnormalize);
00294 
00297   void attemptTrajectory(std::string arm_name, const std::vector< std::vector<double> > &positions,
00298                          bool unnormalize, float time_per_segment);
00299 
00301   trajectory_msgs::JointTrajectory assembleJointTrajectory(std::string arm_name, 
00302                          const std::vector< std::vector<double> > &positions, 
00303                          float time_per_segment);
00304 
00306   void unnormalizeTrajectory(std::string arm_name, const trajectory_msgs::JointTrajectory &input_trajectory,
00307                            trajectory_msgs::JointTrajectory &normalized_trajectory);
00308 
00309   //---------- move arm ----------
00310 
00312   bool attemptMoveArmToGoal(std::string arm_name, const std::vector<double> &desired_joint_values,
00313                             const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00314                             const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00315                             int max_tries = 5, bool reset_map_if_stuck = true, double timeout = 60);
00316 
00318   void modifyMoveArmGoal(arm_navigation_msgs::MoveArmGoal &move_arm_goal, 
00319                          arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
00320                          std::vector<arm_navigation_msgs::ContactInformation> &contact_info_);
00321 
00323   void cancelMoveArmGoal(std::string arm_name);
00324 
00326   actionlib::SimpleClientGoalState getMoveArmState(std::string arm_name);
00327 
00329   bool clearMoveArmGoals(double timeout = 30);
00330 
00332   bool moveArmToPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00333                      const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00334                      const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);
00335 
00337   bool moveArmConstrained(std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose,
00338                           const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
00339                           const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00340                           const arm_navigation_msgs::Constraints &path_constraints,
00341                           const double &redundancy = 0.0,
00342                           const bool &compute_viable_pose = true);
00343 
00344   //---------- gripper ----------
00345 
00347   void handPostureGraspAction(std::string arm_name, const object_manipulation_msgs::Grasp &grasp, int goal, float max_contact_force);
00348 
00350   bool graspPostureQuery(std::string arm_name, const object_manipulation_msgs::Grasp grasp);
00351 
00352   //---------- head ----------
00353 
00355   bool pointHeadAction(const geometry_msgs::PointStamped &target, 
00356                        std::string pointing_frame="/narrow_stereo_optical_frame", bool wait_for_result = true);
00357 
00358   //------------- tf -------------
00359 
00361   geometry_msgs::PoseStamped getGripperPose(std::string arm_name, std::string frame_id);
00362 
00364   geometry_msgs::PoseStamped translateGripperPose(geometry_msgs::Vector3Stamped translation,
00365                                                   geometry_msgs::PoseStamped start_pose,
00366                                                   std::string arm_id);
00367 
00369   geometry_msgs::PoseStamped transformPose(const std::string target_frame, 
00370                                            const geometry_msgs::PoseStamped &stamped_in);
00371 
00373   void transformPointCloud(std::string target_frame, 
00374                            const sensor_msgs::PointCloud &cloud_in,
00375                            sensor_msgs::PointCloud &cloud_out);
00376 
00378   geometry_msgs::PoseStamped getObjectPoseForGrasp(std::string arm_name, 
00379                                                    const geometry_msgs::Pose &grasp_pose);
00380 
00384   void convertGraspableObjectComponentsToFrame(object_manipulation_msgs::GraspableObject &object,
00385                                                std::string frame_id);
00386 
00388   float vectorLen(const geometry_msgs::Vector3 &vec)
00389   {
00390     tf::Vector3 v;
00391     tf::vector3MsgToTF(vec, v);
00392     return v.length();
00393   }
00394 
00396   geometry_msgs::Vector3 normalize(const geometry_msgs::Vector3 &vec)
00397   {
00398     tf::Vector3 v;
00399     tf::vector3MsgToTF(vec, v);
00400     v.normalize();
00401     geometry_msgs::Vector3 vr;
00402     tf::vector3TFToMsg(v, vr);
00403     return vr;
00404   }
00405 
00406   geometry_msgs::Vector3 negate(const geometry_msgs::Vector3 &vec)
00407   {
00408     geometry_msgs::Vector3 v;
00409     v.x = - vec.x;
00410     v.y = - vec.y;
00411     v.z = - vec.z;
00412     return v;
00413   }
00414 
00415   //------------ controller switching ------------
00416 
00418   bool checkController(std::string controller);
00419 
00421   bool switchControllers(std::string start_controller, std::string stop_controller);
00422 
00424   bool startController(std::string controller);
00425 
00427   bool stopController(std::string controller);
00428 
00430   bool switchToCartesian(std::string arm_name);
00431 
00433   bool switchToJoint(std::string arm_name);
00434 
00436   std::string cartesianControllerName(std::string arm_name);
00437 
00439   std::string jointControllerName(std::string arm_name);
00440 
00441   //------------ Cartesian movement and related helper functions ------------
00442 
00444   void sendCartesianPoseCommand(std::string arm_name, geometry_msgs::PoseStamped desired_pose);
00445 
00447   void sendCartesianPostureCommand(std::string arm_name, std::vector<double> arm_angles);
00448 
00449   /*
00451   void sendCartesianGainsCommand(std::string arm_name, std::vector<double> gains, std::string frame_id, std::vector<double> fixed_frame);
00452   */
00453 
00455   void setCartesianPostureGoalToCurrentAngles(std::string arm_name);
00456 
00458   // (to within tolerances) or times out (returns 0 for error, 1 for got there, -1 for timed out)
00459   int moveArmToPoseCartesian(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
00460                               ros::Duration timeout, double dist_tol = 0.001, double angle_tol = 0.05,
00461                              double clip_dist = .02, double clip_angle = .16, 
00462                              double overshoot_dist = .005, double overshoot_angle = .087,
00463                              double timestep = 0.1,
00464                              const std::vector<double> &goal_posture_suggestion = std::vector<double>());
00465 
00467   int translateGripperCartesian(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
00468                                 ros::Duration timeout, double dist_tol = .001, double angle_tol = .05,
00469                                 double clip_dist = .02, double clip_angle = .16, 
00470                                 double overshoot_dist = 0.005, double overshoot_angle = 0.087, double timestep = 0.1);
00471 
00473   void poseDists(geometry_msgs::Pose start, geometry_msgs::Pose end, double &pos_dist, double &angle);
00474 
00476   void positionAndAngleDist(Eigen::Affine3d start, Eigen::Affine3d end, double &pos_dist, 
00477                                                 double &angle, Eigen::Vector3d &axis, Eigen::Vector3d &direction);
00478 
00480   geometry_msgs::PoseStamped clipDesiredPose(const geometry_msgs::PoseStamped &current_pose,
00481                                                                  const geometry_msgs::PoseStamped &desired_pose,
00482                                                                  double clip_dist, double clip_angle,
00483                                                                  double &resulting_clip_fraction);
00484 
00486   geometry_msgs::PoseStamped clipDesiredPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, 
00487                                              double clip_dist, double clip_angle, double &resulting_clip_fraction);
00488 
00490   // and clip an optional goal posture suggestion (arm angles) by the same amount
00491   geometry_msgs::PoseStamped clipDesiredPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, 
00492                                              double clip_dist, double clip_angle, double &resulting_clip_fraction,
00493                                              const std::vector<double> &goal_posture_suggestion,
00494                                              std::vector<double> &clipped_posture_goal);
00495 
00497   geometry_msgs::PoseStamped overshootDesiredPose(std::string arm_name, 
00498                                              const geometry_msgs::PoseStamped &desired_pose, 
00499                                              double overshoot_dist, double overshoot_angle, 
00500                                              double dist_tol, double angle_tol);
00501 
00503   std::vector<std::string> getJointNames(std::string arm_name);
00504 
00506   bool getArmAngles(std::string arm_name, std::vector<double> &arm_angles);
00507 
00508   //------------ misc ------------
00509 
00511   bool translateGripper(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
00512                         arm_navigation_msgs::OrderedCollisionOperations ord, 
00513                         const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
00514                         float requested_distance, float min_distance,
00515                         float &actual_distance);
00516 
00518   static std::vector<arm_navigation_msgs::LinkPadding> fingertipPadding(std::string arm_name, double pad);
00519 
00521   static std::vector<arm_navigation_msgs::LinkPadding> gripperPadding(std::string arm_name, double pad);
00522 
00523   //------------ attached objects (collision space) ------------
00524 
00526   void attachObjectToGripper(std::string arm_name, std::string object_collision_name);
00527 
00529   void detachAndAddBackObjectsAttachedToGripper(std::string arm_name, std::string object_collision_name);
00530 
00532   void detachAllObjectsFromGripper(std::string arm_name);
00533 };
00534 
00536 
00541 inline MechanismInterface& mechInterface()
00542 {
00543   static MechanismInterface mech_interface;
00544   return mech_interface;
00545 }
00546 
00547 } //namespace object_manipulator
00548 
00549 #endif


object_manipulator
Author(s): Matei Ciocarlie and Kaijen Hsiao
autogenerated on Thu Jan 2 2014 11:39:04