mechanism_interface.h

Go to the documentation of this file.

/*********************************************************************
*
*  Copyright (c) 2009, Willow Garage, Inc.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Willow Garage nor the names of its
*     contributors may be used to endorse or promote products derived
*     from this software without specific prior written permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*********************************************************************/
#ifndef _MECHANISM_INTERFACE_H_
#define _MECHANISM_INTERFACE_H_

#include <ros/ros.h>

#include <actionlib/client/simple_action_client.h>

#include <tf/transform_listener.h>

#include <eigen_conversions/eigen_msg.h>
#include <Eigen/Core>
#include <Eigen/Geometry>

#include <geometry_msgs/Vector3.h>

#include <kinematics_msgs/GetKinematicSolverInfo.h>
#include <kinematics_msgs/GetConstraintAwarePositionIK.h>
#include <kinematics_msgs/GetPositionFK.h>

#include <arm_navigation_msgs/MoveArmAction.h>

#include <arm_navigation_msgs/GetMotionPlan.h>
#include <arm_navigation_msgs/convert_messages.h>
#include <arm_navigation_msgs/FilterJointTrajectory.h>
#include <arm_navigation_msgs/OrderedCollisionOperations.h>

#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <pr2_controllers_msgs/PointHeadAction.h>

#include <arm_navigation_msgs/ContactInformation.h>
#include <arm_navigation_msgs/GetStateValidity.h>
#include <arm_navigation_msgs/SetPlanningSceneDiff.h>
#include <arm_navigation_msgs/GetRobotState.h>

#include <std_srvs/Empty.h>

#include <interpolated_ik_motion_planner/SetInterpolatedIKMotionPlanParams.h>

#include <arm_navigation_msgs/AttachedCollisionObject.h>

#include <planning_environment/models/collision_models.h>

#include <object_manipulation_msgs/ReactiveGraspAction.h>
#include <object_manipulation_msgs/ReactiveLiftAction.h>
#include <object_manipulation_msgs/ReactivePlaceAction.h>
#include <object_manipulation_msgs/GraspHandPostureExecutionAction.h>
#include <object_manipulation_msgs/GraspStatus.h>

#include <pr2_mechanism_msgs/SwitchController.h>
#include <pr2_mechanism_msgs/ListControllers.h>

#include "object_manipulator/tools/exceptions.h"
#include "object_manipulator/tools/service_action_wrappers.h"


namespace object_manipulator {

class MechanismInterface
{
 private:
  ros::NodeHandle root_nh_;

  ros::NodeHandle priv_nh_;

  tf::TransformListener listener_;

  ros::Publisher attached_object_pub_;

  std::string joint_states_topic_;

  std::map<std::string, std::string> joint_controller_names_; 

  std::map<std::string, std::string> cartesian_controller_names_; 

  arm_navigation_msgs::OrderedCollisionOperations collision_operations_cache_;

  std::vector<arm_navigation_msgs::LinkPadding> link_padding_cache_;

  planning_environment::CollisionModels cm_;
  planning_models::KinematicState* planning_scene_state_;

  bool planning_scene_cache_empty_;

  bool cache_planning_scene_;

  void setInterpolatedIKParams(std::string arm_name, int num_steps, 
                               int collision_check_resolution, bool start_from_end);

  bool callSwitchControllers(std::vector<std::string> start_controllers, std::vector<std::string> stop_controllers);
  

  bool cachePlanningScene(const arm_navigation_msgs::OrderedCollisionOperations& collision_operations,
                          const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);

 public:

  //----------------------------- Service clients -------------------------------

  MultiArmServiceWrapper<kinematics_msgs::GetKinematicSolverInfo> ik_query_client_;

  MultiArmServiceWrapper<kinematics_msgs::GetConstraintAwarePositionIK> ik_service_client_;

  MultiArmServiceWrapper<kinematics_msgs::GetPositionFK> fk_service_client_;

  MultiArmServiceWrapper<arm_navigation_msgs::GetMotionPlan> interpolated_ik_service_client_;  

  MultiArmServiceWrapper<interpolated_ik_motion_planner::SetInterpolatedIKMotionPlanParams> 
    interpolated_ik_set_params_client_;

  MultiArmServiceWrapper<object_manipulation_msgs::GraspStatus> grasp_status_client_;

  ServiceWrapper<arm_navigation_msgs::GetStateValidity> check_state_validity_client_;

  ServiceWrapper<arm_navigation_msgs::FilterJointTrajectory> joint_trajectory_normalizer_service_;
  
  ServiceWrapper<pr2_mechanism_msgs::SwitchController> switch_controller_service_;

  ServiceWrapper<pr2_mechanism_msgs::ListControllers> list_controllers_service_;

  ServiceWrapper<arm_navigation_msgs::GetRobotState> get_robot_state_client_;

  ServiceWrapper<arm_navigation_msgs::SetPlanningSceneDiff> set_planning_scene_diff_service_;

  ServiceWrapper<std_srvs::Empty> reset_collision_map_service_;

  //------------------------------ Action clients -------------------------------

  MultiArmActionWrapper<object_manipulation_msgs::ReactiveGraspAction> reactive_grasp_action_client_;

  MultiArmActionWrapper<object_manipulation_msgs::ReactiveLiftAction> reactive_lift_action_client_;

  MultiArmActionWrapper<object_manipulation_msgs::ReactivePlaceAction> reactive_place_action_client_;

  MultiArmActionWrapper<arm_navigation_msgs::MoveArmAction> move_arm_action_client_;  

  MultiArmActionWrapper<pr2_controllers_msgs::JointTrajectoryAction> traj_action_client_;

  MultiArmActionWrapper<object_manipulation_msgs::GraspHandPostureExecutionAction> hand_posture_client_;

  ActionWrapper<pr2_controllers_msgs::PointHeadAction> point_head_action_client_;

  //------------------------------ Publishers ----------------------------------

  MultiArmTopicWrapper<geometry_msgs::PoseStamped> cartesian_pub_;

  MultiArmTopicWrapper<std_msgs::Float64MultiArray> cartesian_posture_pub_;

  //MultiArmTopicWrapper<std_msgs::Float64MultiArray> cartesian_gains_pub_;
  //MultiArmTopicWrapper<std_msgs::CartesianGains> cartesian_gains_pub_;

  //------------------------------ Functionality -------------------------------

  MechanismInterface();

  ~MechanismInterface() {
    if(planning_scene_state_ != NULL) {
      cm_.revertPlanningScene(planning_scene_state_);
    }
  }

  planning_environment::CollisionModels& getCollisionModels() {
    return cm_;
  }
  
  planning_models::KinematicState* getPlanningSceneState() const {
    return planning_scene_state_;
  }

  //------------- IK -------------

  void getRobotState(arm_navigation_msgs::RobotState& state);

  void getPlanningScene(const arm_navigation_msgs::OrderedCollisionOperations& collision_operations,
                        const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);

  bool checkStateValidity(std::string arm_name, const std::vector<double> &joint_values,
                          const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
                          const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);

  bool checkStateValidity(std::string arm_name, const std::vector<double> &joint_values)
  {
    arm_navigation_msgs::OrderedCollisionOperations empty;
    std::vector<arm_navigation_msgs::LinkPadding> also_empty;
    return checkStateValidity(arm_name, joint_values, empty, also_empty);
  }

  bool getIKForPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
                    kinematics_msgs::GetConstraintAwarePositionIK::Response& ik_response,
                    const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
                    const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);

  bool getFK(std::string arm_name, 
             std::vector<double> positions, 
             geometry_msgs::PoseStamped &pose_stamped);


  int getInterpolatedIK(std::string arm_name,
                        geometry_msgs::PoseStamped start_pose,
                        geometry_msgs::Vector3Stamped direction,
                        float desired_trajectory_length,
                        const std::vector<double> &seed_joint_position,
                        const sensor_msgs::JointState &joint_state,
                        const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
                        const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
                        bool reverse_trajectory,
                        trajectory_msgs::JointTrajectory &trajectory,
                        float &actual_trajectory_length);

  //------ traj controller  ------

  void attemptTrajectory(std::string arm_name, const trajectory_msgs::JointTrajectory &trajectory, bool unnormalize);

  void attemptTrajectory(std::string arm_name, const std::vector< std::vector<double> > &positions,
                         bool unnormalize, float time_per_segment);

  trajectory_msgs::JointTrajectory assembleJointTrajectory(std::string arm_name, 
                         const std::vector< std::vector<double> > &positions, 
                         float time_per_segment);

  void unnormalizeTrajectory(std::string arm_name, const trajectory_msgs::JointTrajectory &input_trajectory,
                           trajectory_msgs::JointTrajectory &normalized_trajectory);

  //---------- move arm ----------

  bool attemptMoveArmToGoal(std::string arm_name, const std::vector<double> &desired_joint_values,
                            const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
                            const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
                            int max_tries = 5, bool reset_map_if_stuck = true, double timeout = 60);

  void modifyMoveArmGoal(arm_navigation_msgs::MoveArmGoal &move_arm_goal, 
                         arm_navigation_msgs::ArmNavigationErrorCodes &error_code,
                         std::vector<arm_navigation_msgs::ContactInformation> &contact_info_);

  void cancelMoveArmGoal(std::string arm_name);

  actionlib::SimpleClientGoalState getMoveArmState(std::string arm_name);

  bool clearMoveArmGoals(double timeout = 30);

  bool moveArmToPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
                     const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
                     const std::vector<arm_navigation_msgs::LinkPadding> &link_padding);

  bool moveArmConstrained(std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose,
                          const arm_navigation_msgs::OrderedCollisionOperations &collision_operations,
                          const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
                          const arm_navigation_msgs::Constraints &path_constraints,
                          const double &redundancy = 0.0,
                          const bool &compute_viable_pose = true);

  //---------- gripper ----------

  void handPostureGraspAction(std::string arm_name, const object_manipulation_msgs::Grasp &grasp, int goal, float max_contact_force);

  bool graspPostureQuery(std::string arm_name, const object_manipulation_msgs::Grasp grasp);

  //---------- head ----------

  bool pointHeadAction(const geometry_msgs::PointStamped &target, 
                       std::string pointing_frame="/narrow_stereo_optical_frame", bool wait_for_result = true);

  //------------- tf -------------

  geometry_msgs::PoseStamped getGripperPose(std::string arm_name, std::string frame_id);

  geometry_msgs::PoseStamped translateGripperPose(geometry_msgs::Vector3Stamped translation,
                                                  geometry_msgs::PoseStamped start_pose,
                                                  std::string arm_id);

  geometry_msgs::PoseStamped transformPose(const std::string target_frame, 
                                           const geometry_msgs::PoseStamped &stamped_in);

  void transformPointCloud(std::string target_frame, 
                           const sensor_msgs::PointCloud &cloud_in,
                           sensor_msgs::PointCloud &cloud_out);

  geometry_msgs::PoseStamped getObjectPoseForGrasp(std::string arm_name, 
                                                   const geometry_msgs::Pose &grasp_pose);

  void convertGraspableObjectComponentsToFrame(object_manipulation_msgs::GraspableObject &object,
                                               std::string frame_id);

  float vectorLen(const geometry_msgs::Vector3 &vec)
  {
    tf::Vector3 v;
    tf::vector3MsgToTF(vec, v);
    return v.length();
  }

  geometry_msgs::Vector3 normalize(const geometry_msgs::Vector3 &vec)
  {
    tf::Vector3 v;
    tf::vector3MsgToTF(vec, v);
    v.normalize();
    geometry_msgs::Vector3 vr;
    tf::vector3TFToMsg(v, vr);
    return vr;
  }

  geometry_msgs::Vector3 negate(const geometry_msgs::Vector3 &vec)
  {
    geometry_msgs::Vector3 v;
    v.x = - vec.x;
    v.y = - vec.y;
    v.z = - vec.z;
    return v;
  }

  //------------ controller switching ------------

  bool checkController(std::string controller);

  bool switchControllers(std::string start_controller, std::string stop_controller);

  bool startController(std::string controller);

  bool stopController(std::string controller);

  bool switchToCartesian(std::string arm_name);

  bool switchToJoint(std::string arm_name);

  std::string cartesianControllerName(std::string arm_name);

  std::string jointControllerName(std::string arm_name);

  //------------ Cartesian movement and related helper functions ------------

  void sendCartesianPoseCommand(std::string arm_name, geometry_msgs::PoseStamped desired_pose);

  void sendCartesianPostureCommand(std::string arm_name, std::vector<double> arm_angles);

  /*
  void sendCartesianGainsCommand(std::string arm_name, std::vector<double> gains, std::string frame_id, std::vector<double> fixed_frame);
  */

  void setCartesianPostureGoalToCurrentAngles(std::string arm_name);

  // (to within tolerances) or times out (returns 0 for error, 1 for got there, -1 for timed out)
  int moveArmToPoseCartesian(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose,
                              ros::Duration timeout, double dist_tol = 0.001, double angle_tol = 0.05,
                             double clip_dist = .02, double clip_angle = .16, 
                             double overshoot_dist = .005, double overshoot_angle = .087,
                             double timestep = 0.1,
                             const std::vector<double> &goal_posture_suggestion = std::vector<double>());

  int translateGripperCartesian(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
                                ros::Duration timeout, double dist_tol = .001, double angle_tol = .05,
                                double clip_dist = .02, double clip_angle = .16, 
                                double overshoot_dist = 0.005, double overshoot_angle = 0.087, double timestep = 0.1);

  void poseDists(geometry_msgs::Pose start, geometry_msgs::Pose end, double &pos_dist, double &angle);

  void positionAndAngleDist(Eigen::Affine3d start, Eigen::Affine3d end, double &pos_dist, 
                                                double &angle, Eigen::Vector3d &axis, Eigen::Vector3d &direction);

  geometry_msgs::PoseStamped clipDesiredPose(const geometry_msgs::PoseStamped &current_pose,
                                                                 const geometry_msgs::PoseStamped &desired_pose,
                                                                 double clip_dist, double clip_angle,
                                                                 double &resulting_clip_fraction);

  geometry_msgs::PoseStamped clipDesiredPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, 
                                             double clip_dist, double clip_angle, double &resulting_clip_fraction);

  // and clip an optional goal posture suggestion (arm angles) by the same amount
  geometry_msgs::PoseStamped clipDesiredPose(std::string arm_name, const geometry_msgs::PoseStamped &desired_pose, 
                                             double clip_dist, double clip_angle, double &resulting_clip_fraction,
                                             const std::vector<double> &goal_posture_suggestion,
                                             std::vector<double> &clipped_posture_goal);

  geometry_msgs::PoseStamped overshootDesiredPose(std::string arm_name, 
                                             const geometry_msgs::PoseStamped &desired_pose, 
                                             double overshoot_dist, double overshoot_angle, 
                                             double dist_tol, double angle_tol);

  std::vector<std::string> getJointNames(std::string arm_name);

  bool getArmAngles(std::string arm_name, std::vector<double> &arm_angles);

  //------------ misc ------------

  bool translateGripper(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
                        arm_navigation_msgs::OrderedCollisionOperations ord, 
                        const std::vector<arm_navigation_msgs::LinkPadding> &link_padding,
                        float requested_distance, float min_distance,
                        float &actual_distance);

  static std::vector<arm_navigation_msgs::LinkPadding> fingertipPadding(std::string arm_name, double pad);

  static std::vector<arm_navigation_msgs::LinkPadding> gripperPadding(std::string arm_name, double pad);

  //------------ attached objects (collision space) ------------

  void attachObjectToGripper(std::string arm_name, std::string object_collision_name);

  void detachAndAddBackObjectsAttachedToGripper(std::string arm_name, std::string object_collision_name);

  void detachAllObjectsFromGripper(std::string arm_name);
};


inline MechanismInterface& mechInterface()
{
  static MechanismInterface mech_interface;
  return mech_interface;
}

} //namespace object_manipulator

#endif