ompl_ros_interface: ompl_ros_interface: ompl_ros

00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2008, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of Willow Garage, Inc. nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00037 #ifndef OMPL_ROS_CONVERSIONS_
00038 #define OMPL_ROS_CONVERSIONS_
00039 
00040 #include <ros/ros.h>
00041 #include <ros/console.h>
00042 #include <angles/angles.h>
00043 
00044 #include <ompl/base/spaces/SO2StateSpace.h>
00045 #include <ompl/base/spaces/SO3StateSpace.h>
00046 #include <ompl/base/spaces/SE2StateSpace.h>
00047 #include <ompl/base/spaces/SE3StateSpace.h>
00048 #include <ompl/base/spaces/RealVectorStateSpace.h>
00049 #include <ompl/geometric/PathGeometric.h>
00050 #include <ompl/base/StateSpace.h>
00051 #include <ompl/base/ScopedState.h>
00052 #include <ompl/base/State.h>
00053 
00054 #include <planning_models/kinematic_model.h>
00055 #include <planning_models/kinematic_state.h>
00056 #include <arm_navigation_msgs/convert_messages.h>
00057 #include <arm_navigation_msgs/RobotState.h>
00058 #include <arm_navigation_msgs/RobotTrajectory.h>
00059 #include <arm_navigation_msgs/JointConstraint.h>
00060 #include <sensor_msgs/JointState.h>
00061 
00062 namespace ompl_ros_interface
00063 {
00064 
00068 typedef enum{REAL_VECTOR, SO2, SO3, SE2, SE3, COMPOUND, UNKNOWN}MAPPING_TYPE;
00069 
00074 class OmplStateToRobotStateMapping
00075 {
00076 public:
00077   OmplStateToRobotStateMapping():real_vector_index(-1){}
00078   std::vector<int> ompl_state_mapping;
00079   int real_vector_index;
00080   std::vector<int> real_vector_mapping;
00081   std::vector<ompl_ros_interface::MAPPING_TYPE> mapping_type;
00082   unsigned int num_joints;
00083 };
00084 
00089 class RobotStateToOmplStateMapping
00090 {
00091 public:
00092   RobotStateToOmplStateMapping():real_vector_index(-1){}
00093   std::vector<int> multi_dof_mapping;
00094   int real_vector_index;
00095   std::vector<int> joint_state_mapping;
00096   std::vector<ompl_ros_interface::MAPPING_TYPE> joint_mapping_type;
00097   std::vector<ompl_ros_interface::MAPPING_TYPE> multi_dof_joint_mapping_type;
00098 };
00099 
00104 class KinematicStateToOmplStateMapping
00105 {
00106 public:
00107   KinematicStateToOmplStateMapping():real_vector_index(-1){}
00108   int real_vector_index;
00109   std::vector<unsigned int> joint_state_mapping;
00110   std::vector<ompl_ros_interface::MAPPING_TYPE> joint_mapping_type;
00111 };
00112 
00117 class OmplStateToKinematicStateMapping
00118 {
00119 public:
00120   OmplStateToKinematicStateMapping():real_vector_index(-1){}
00121   int real_vector_index;
00122   std::vector<unsigned int> ompl_state_mapping;
00123   std::vector<unsigned int> real_vector_mapping;
00124   std::vector<ompl_ros_interface::MAPPING_TYPE> mapping_type;
00125 };
00126 
00131 class RobotStateToKinematicStateMapping
00132 {
00133 public:
00134   std::vector<int> multi_dof_mapping;
00135   std::vector<int> joint_state_mapping;
00136 };
00137 
00138 
00139 ompl_ros_interface::MAPPING_TYPE getMappingType(const planning_models::KinematicModel::JointModel *joint_model);
00140 
00141 ompl_ros_interface::MAPPING_TYPE getMappingType(const ompl::base::StateSpace *state_space);
00142 
00143 ompl_ros_interface::MAPPING_TYPE getMappingType(const ompl::base::StateSpacePtr &state_space);
00144 
00153 ompl::base::StateSpacePtr jointGroupToOmplStateSpacePtr(const planning_models::KinematicModel::JointModelGroup *joint_group, 
00154                                                               ompl_ros_interface::OmplStateToKinematicStateMapping &ompl_kinematic_mapping,
00155                                                               ompl_ros_interface::KinematicStateToOmplStateMapping &kinematic_ompl_mapping);
00156 
00165 bool getRobotStateToOmplStateMapping(const arm_navigation_msgs::RobotState &robot_state, 
00166                                      const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00167                                      ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00168                                      const bool &fail_if_match_not_found = true);
00169 
00178 bool getJointStateToOmplStateMapping(const sensor_msgs::JointState &joint_state, 
00179                                      const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00180                                      ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00181                                      const bool &fail_if_match_not_found = true);
00182 
00191 bool getOmplStateToJointStateMapping(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00192                                      const sensor_msgs::JointState &joint_state,
00193                                      ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00194                                      const bool &fail_if_match_not_found = true);
00195 
00204 bool getOmplStateToRobotStateMapping(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00205                                      const arm_navigation_msgs::RobotState &robot_state, 
00206                                      ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00207                                      const bool &fail_if_match_not_found = true);
00208 
00217 bool omplStateToRobotState(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00218                            const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00219                            arm_navigation_msgs::RobotState &robot_state);
00220 
00229 bool omplStateToRobotState(const ompl::base::State &ompl_state,
00230                            const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00231                            arm_navigation_msgs::RobotState &robot_state);
00232 
00241 bool omplRealVectorStateToJointState(const ompl::base::RealVectorStateSpace::StateType &ompl_state,
00242                                      const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00243                                      sensor_msgs::JointState &joint_state);
00244 
00253 bool robotStateToOmplState(const arm_navigation_msgs::RobotState &robot_state,
00254                            const ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00255                            ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00256                            const bool &fail_if_match_not_found = true);
00257 
00266 bool robotStateToOmplState(const arm_navigation_msgs::RobotState &robot_state,
00267                            const ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00268                            ompl::base::State *ompl_state,
00269                            const bool &fail_if_match_not_found = true);
00270 
00278 bool robotStateToOmplState(const arm_navigation_msgs::RobotState &robot_state,
00279                            ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00280                            const bool &fail_if_match_not_found = true);
00281 
00282 
00291 bool jointStateToRealVectorState(const sensor_msgs::JointState &joint_state,
00292                                  const ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00293                                  ompl::base::RealVectorStateSpace::StateType &real_vector_state,
00294                                  const bool &fail_if_match_not_found = true);
00295 
00301 void SE3StateSpaceToPoseMsg(const ompl::base::SE3StateSpace::StateType &pose,
00302                           geometry_msgs::Pose &pose_msg);
00303 
00309 void SO3StateSpaceToQuaternionMsg(const ompl::base::SO3StateSpace::StateType &quaternion,
00310                                 geometry_msgs::Quaternion &quaternion_msg);
00311 
00317 void SO3StateSpaceToPoseMsg(const ompl::base::SO3StateSpace::StateType &quaternion,
00318                           geometry_msgs::Pose &pose_msg);
00319 
00325 void SE2StateSpaceToPoseMsg(const ompl::base::SE2StateSpace::StateType &pose,
00326                           geometry_msgs::Pose &pose_msg);
00327 
00333 void poseMsgToSE3StateSpace(const geometry_msgs::Pose &pose_msg,
00334                           ompl::base::SE3StateSpace::StateType &pose);
00335 
00341 void quaternionMsgToSO3StateSpace(const geometry_msgs::Quaternion &quaternion_msg,
00342                                 ompl::base::SO3StateSpace::StateType &quaternion);
00343 
00344 
00351 bool getJointStateGroupToOmplStateMapping(planning_models::KinematicState::JointStateGroup* joint_state_group, 
00352                                           const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00353                                           ompl_ros_interface::KinematicStateToOmplStateMapping &mapping);
00354 
00355 
00362 bool getOmplStateToJointStateGroupMapping(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00363                                           planning_models::KinematicState::JointStateGroup* joint_state_group,
00364                                           ompl_ros_interface::OmplStateToKinematicStateMapping &mapping);
00365 
00372 bool kinematicStateGroupToOmplState(const planning_models::KinematicState::JointStateGroup* joint_state_group, 
00373                                     const ompl_ros_interface::KinematicStateToOmplStateMapping &mapping,
00374                                     ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state);
00375 
00382 bool omplStateToKinematicStateGroup(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_state,
00383                                     const ompl_ros_interface::OmplStateToKinematicStateMapping &mapping,
00384                                     planning_models::KinematicState::JointStateGroup* joint_state_group);
00385 
00392 bool omplStateToKinematicStateGroup(const ompl::base::State *ompl_state,
00393                                     const ompl_ros_interface::OmplStateToKinematicStateMapping &mapping,
00394                                     planning_models::KinematicState::JointStateGroup* joint_state_group);
00395 
00402 bool jointStateGroupToRobotTrajectory(planning_models::KinematicState::JointStateGroup* joint_state_group, 
00403                                       arm_navigation_msgs::RobotTrajectory &robot_trajectory);
00404 
00412 bool omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path,
00413                                         const ompl::base::StateSpacePtr &state_space,
00414                                         arm_navigation_msgs::RobotTrajectory &robot_trajectory);
00415 
00423 bool omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path,
00424                                         const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00425                                         arm_navigation_msgs::RobotTrajectory &robot_trajectory);
00426 
00434 bool getOmplStateToRobotTrajectoryMapping(const ompl::base::StateSpacePtr &state_space,
00435                                           const arm_navigation_msgs::RobotTrajectory &robot_trajectory, 
00436                                           ompl_ros_interface::OmplStateToRobotStateMapping &mapping);
00437 
00445 bool getOmplStateToJointTrajectoryMapping(const ompl::base::StateSpacePtr &state_space,
00446                                           const trajectory_msgs::JointTrajectory &joint_trajectory,
00447                                           ompl_ros_interface::OmplStateToRobotStateMapping &mapping);
00448 
00455 bool jointConstraintsToOmplState(const std::vector<arm_navigation_msgs::JointConstraint> &joint_constraints,
00456                                  ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state);
00457 
00464 bool jointConstraintsToOmplState(const std::vector<arm_navigation_msgs::JointConstraint> &joint_constraints,
00465                                  ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state);
00466 
00474 bool constraintsToOmplState(const arm_navigation_msgs::Constraints &constraints,
00475                             ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00476                             const bool &fail_if_match_not_found = true);
00477 
00485 bool robotStateToJointStateGroup(const arm_navigation_msgs::RobotState &robot_state,
00486                                  const ompl_ros_interface::RobotStateToKinematicStateMapping &robot_state_to_joint_state_group_mapping,
00487                                  planning_models::KinematicState::JointStateGroup *joint_state_group);
00488 
00496 bool getRobotStateToJointModelGroupMapping(const arm_navigation_msgs::RobotState &robot_state,
00497                                            const planning_models::KinematicModel::JointModelGroup *joint_model_group,
00498                                            ompl_ros_interface::RobotStateToKinematicStateMapping &mapping);
00499 
00507 bool addToOmplStateSpace(const planning_models::KinematicModel* kinematic_model, 
00508                          const std::string &joint_name,
00509                          ompl::base::StateSpacePtr &ompl_state_space);
00510 
00511 }
00512 #endif
ompl_ros_conversions.h
