00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
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
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
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
00212
00214 MultiArmTopicWrapper<geometry_msgs::PoseStamped> cartesian_pub_;
00215
00217 MultiArmTopicWrapper<std_msgs::Float64MultiArray> cartesian_posture_pub_;
00218
00220
00221
00222
00223
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
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
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
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
00345
00347 void handPostureGraspAction(std::string arm_name, const manipulation_msgs::Grasp &grasp, int goal, float max_contact_force);
00348
00350 bool graspPostureQuery(std::string arm_name, const manipulation_msgs::Grasp grasp);
00351
00352
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
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(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
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
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
00452
00453
00455 void setCartesianPostureGoalToCurrentAngles(std::string arm_name);
00456
00458
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 ¤t_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
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
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
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 }
00548
00549 #endif