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 <move_arm_msgs/MoveArmAction.h>

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

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

#include <planning_environment_msgs/ContactInformation.h>
#include <planning_environment_msgs/GetStateValidity.h>

#include <interpolated_ik_motion_planner/SetInterpolatedIKMotionPlanParams.h>

#include <mapping_msgs/AttachedCollisionObject.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::vector<std::string> getJointNames(std::string arm_name);

  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);

  std::string right_cartesian_controller_;
  std::string left_cartesian_controller_;
  std::string right_joint_controller_;
  std::string left_joint_controller_;

 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<motion_planning_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<planning_environment_msgs::GetStateValidity> check_state_validity_client_;

  ServiceWrapper<motion_planning_msgs::FilterJointTrajectory> joint_trajectory_normalizer_service_;

  ServiceWrapper<pr2_mechanism_msgs::SwitchController> switch_controller_service_;

  ServiceWrapper<pr2_mechanism_msgs::ListControllers> list_controllers_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<move_arm_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 ----------------------------------

  ros::Publisher right_cartesian_pub_;
  ros::Publisher left_cartesian_pub_;

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

  MechanismInterface();

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

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

  bool checkStateValidity(std::string arm_name, const std::vector<double> &joint_values)
  {
    motion_planning_msgs::OrderedCollisionOperations empty;
    std::vector<motion_planning_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 motion_planning_msgs::OrderedCollisionOperations &collision_operations,
                    const std::vector<motion_planning_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 motion_planning_msgs::OrderedCollisionOperations &collision_operations,
                        const std::vector<motion_planning_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);

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

  bool attemptMoveArmToGoal(std::string arm_name, const std::vector<double> &desired_joint_values,
                            const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
                            const std::vector<motion_planning_msgs::LinkPadding> &link_padding);

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

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

  bool moveArmConstrained(std::string arm_name, const geometry_msgs::PoseStamped &commanded_pose,
                          const motion_planning_msgs::OrderedCollisionOperations &collision_operations,
                          const std::vector<motion_planning_msgs::LinkPadding> &link_padding,
                          const motion_planning_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);

  bool graspPostureQuery(std::string arm_name);

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

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

  //------------- 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)
  {
    btVector3 v;
    tf::vector3MsgToTF(vec, v);
    return v.length();
  }

  geometry_msgs::Vector3 normalize(const geometry_msgs::Vector3 &vec)
  {
    btVector3 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();

  bool switchToJoint();

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

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

  // (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, double angle_tol,
                              double clip_dist, double clip_angle, double timestep);

  int translateGripperCartesian(std::string arm_name, const geometry_msgs::Vector3Stamped &direction,
                                ros::Duration timeout, double dist_tol, double angle_tol,
                                double clip_dist, double clip_angle, double timestep);

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

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

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

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

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

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

  static std::vector<motion_planning_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);

};


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

} //namespace object_manipulator

#endif