ompl_ros_interface: ompl_ros_conversions.cpp Source File

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 #include <ompl_ros_interface/helpers/ompl_ros_conversions.h>
00038 
00039 namespace ompl_ros_interface
00040 {
00041 ompl::base::StateSpacePtr jointGroupToOmplStateSpacePtr(const planning_models::KinematicModel::JointModelGroup *joint_group, 
00042                                                               ompl_ros_interface::OmplStateToKinematicStateMapping &ompl_kinematic_mapping,
00043                                                               ompl_ros_interface::KinematicStateToOmplStateMapping &kinematic_ompl_mapping)
00044 {
00045   ompl::base::StateSpacePtr ompl_state_space;
00046   ompl_state_space.reset(dynamic_cast<ompl::base::StateSpace*>(new ompl::base::CompoundStateSpace()));
00047   ompl::base::CompoundStateSpace* state_space = dynamic_cast<ompl::base::CompoundStateSpace*> (ompl_state_space.get());
00048 
00049         ompl::base::RealVectorBounds real_vector_bounds(0); // initially assume dimension of R(K) subspace is 0.
00050   ompl::base::RealVectorStateSpace real_vector_joints(0);
00051 
00052   std::vector<std::string> real_vector_names;
00053   std::vector<const planning_models::KinematicModel::JointModel*> joint_models = joint_group->getJointModels();
00054 
00055         for (unsigned int i = 0 ; i < joint_models.size() ; ++i)
00056   {
00057                 const planning_models::KinematicModel::RevoluteJointModel* revolute_joint = 
00058                         dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(joint_models[i]);
00059     if (revolute_joint && revolute_joint->continuous_)
00060     {
00061       ompl::base::SO2StateSpace *subspace = new ompl::base::SO2StateSpace();
00062       subspace->setName(revolute_joint->getName());
00063       state_space->addSubSpace(ompl::base::StateSpacePtr(subspace), 1.0);
00064 
00065       kinematic_ompl_mapping.joint_state_mapping.push_back(state_space->getSubSpaceCount()-1);
00066       kinematic_ompl_mapping.joint_mapping_type.push_back(ompl_ros_interface::SO2);
00067 
00068       ompl_kinematic_mapping.ompl_state_mapping.push_back(i);
00069       ompl_kinematic_mapping.mapping_type.push_back(ompl_ros_interface::SO2);
00070       ROS_DEBUG("Adding SO2 state space with name %s",revolute_joint->getName().c_str());
00071     }
00072     else
00073     {
00074       const planning_models::KinematicModel::PlanarJointModel* planar_joint = 
00075         dynamic_cast<const planning_models::KinematicModel::PlanarJointModel*>(joint_models[i]);
00076       if (planar_joint)
00077       {
00078         ompl::base::SE2StateSpace *subspace = new ompl::base::SE2StateSpace();
00079         subspace->setName(planar_joint->getName());
00080         state_space->addSubSpace(ompl::base::StateSpacePtr(subspace), 1.0);
00081 
00082         kinematic_ompl_mapping.joint_state_mapping.push_back(state_space->getSubSpaceCount()-1);
00083         kinematic_ompl_mapping.joint_mapping_type.push_back(ompl_ros_interface::SE2);
00084 
00085         ompl_kinematic_mapping.ompl_state_mapping.push_back(i);
00086         ompl_kinematic_mapping.mapping_type.push_back(ompl_ros_interface::SE2);
00087       }
00088       else
00089       {
00090         const planning_models::KinematicModel::FloatingJointModel* floating_joint = 
00091           dynamic_cast<const planning_models::KinematicModel::FloatingJointModel*>(joint_models[i]);
00092         if (floating_joint)
00093         {
00094           ompl::base::SE3StateSpace *subspace = new ompl::base::SE3StateSpace();
00095           subspace->setName(floating_joint->getName());
00096           state_space->addSubSpace(ompl::base::StateSpacePtr(subspace), 1.0);
00097 
00098           kinematic_ompl_mapping.joint_state_mapping.push_back(state_space->getSubSpaceCount()-1);
00099           kinematic_ompl_mapping.joint_mapping_type.push_back(ompl_ros_interface::SE3);
00100 
00101           ompl_kinematic_mapping.ompl_state_mapping.push_back(i);
00102           ompl_kinematic_mapping.mapping_type.push_back(ompl_ros_interface::SE3);
00103         }
00104         else
00105         {
00106           // the only other case we consider is R^n; since we know that for now at least the only other type of joint available is non-continuous revolute joints
00107           // we can use the revoluteJoint cast      
00108           std::pair<double,double> bounds = revolute_joint->getVariableBounds(revolute_joint->getName());
00109           real_vector_bounds.low.push_back(bounds.first);
00110           real_vector_bounds.high.push_back(bounds.second);
00111           real_vector_names.push_back(revolute_joint->getName());
00112           kinematic_ompl_mapping.joint_state_mapping.push_back(real_vector_bounds.low.size()-1);
00113           kinematic_ompl_mapping.joint_mapping_type.push_back(ompl_ros_interface::REAL_VECTOR);
00114           ompl_kinematic_mapping.real_vector_mapping.push_back(i);
00115           ROS_DEBUG("Adding real vector joint %s with bounds %f %f",revolute_joint->getName().c_str(),real_vector_bounds.low.back(),real_vector_bounds.high.back());
00116         }
00117       }    
00118     }
00119   }
00120   // we added everything now, except the R(N) state space. we now add R(N) as well, if needed
00121   if (real_vector_bounds.low.size() > 0)
00122   {
00123     ompl::base::RealVectorStateSpace *subspace = new ompl::base::RealVectorStateSpace(real_vector_bounds.low.size());
00124     subspace->setName("real_vector");
00125     subspace->setBounds(real_vector_bounds);
00126     for(unsigned int i=0; i < real_vector_names.size(); i++)
00127       subspace->setDimensionName(i,real_vector_names[i]);
00128     state_space->addSubSpace(ompl::base::StateSpacePtr(subspace),1.0);
00129     kinematic_ompl_mapping.real_vector_index = state_space->getSubSpaceCount()-1;
00130     ompl_kinematic_mapping.real_vector_index = kinematic_ompl_mapping.real_vector_index;
00131     ompl_kinematic_mapping.ompl_state_mapping.push_back(-1);
00132     ompl_kinematic_mapping.mapping_type.push_back(ompl_ros_interface::REAL_VECTOR);
00133   }
00134   for(unsigned int i=0; i < state_space->getSubSpaceCount(); i++)
00135   {
00136     ROS_DEBUG("State state space: subspace %d has name %s",i,state_space->getSubSpace(i)->getName().c_str());
00137   }
00138   return ompl_state_space;
00139 };
00140 
00141 bool addToOmplStateSpace(boost::shared_ptr<planning_models::KinematicModel> kinematic_model, 
00142                             const std::string &joint_name,
00143                             ompl::base::StateSpacePtr &ompl_state_space)
00144 {
00145   ompl::base::CompoundStateSpace* state_space = dynamic_cast<ompl::base::CompoundStateSpace*> (ompl_state_space.get());
00146 
00147   if(!kinematic_model->hasJointModel(joint_name))
00148     { 
00149       ROS_DEBUG("Could not find joint %s",joint_name.c_str());
00150       return false;
00151     }
00152   const planning_models::KinematicModel::JointModel* joint_model = kinematic_model->getJointModel(joint_name);
00153 
00154   const planning_models::KinematicModel::RevoluteJointModel* revolute_joint = 
00155     dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(joint_model);
00156   if (revolute_joint && revolute_joint->continuous_)
00157   {
00158     ompl::base::SO2StateSpace *subspace = new ompl::base::SO2StateSpace();
00159     subspace->setName(revolute_joint->getName());
00160     state_space->addSubSpace(ompl::base::StateSpacePtr(subspace), 1.0);
00161     ROS_DEBUG("Adding SO2 state space with name %s",revolute_joint->getName().c_str());
00162   }
00163   else
00164   {
00165     const planning_models::KinematicModel::PlanarJointModel* planar_joint = 
00166       dynamic_cast<const planning_models::KinematicModel::PlanarJointModel*>(joint_model);
00167     if (planar_joint)
00168     {
00169       ompl::base::SE2StateSpace *subspace = new ompl::base::SE2StateSpace();
00170       subspace->setName(planar_joint->getName());
00171       state_space->addSubSpace(ompl::base::StateSpacePtr(subspace), 1.0);
00172     }
00173     else
00174     {
00175       const planning_models::KinematicModel::FloatingJointModel* floating_joint = 
00176         dynamic_cast<const planning_models::KinematicModel::FloatingJointModel*>(joint_model);
00177       if (floating_joint)
00178       {
00179         ompl::base::SE3StateSpace *subspace = new ompl::base::SE3StateSpace();
00180         subspace->setName(floating_joint->getName());
00181         state_space->addSubSpace(ompl::base::StateSpacePtr(subspace), 1.0);
00182       }
00183       else
00184       {
00185         // the only other case we consider is R^n; since we know that for now at least the only other type of joint available is non-continuous revolute joints
00186         // we can use the revoluteJoint cast      
00187         int real_vector_index = -1;
00188         if(state_space->hasSubSpace("real_vector"))
00189           real_vector_index = state_space->getSubSpaceIndex("real_vector");
00190  
00191         if(real_vector_index < 0)
00192         {
00193           real_vector_index = state_space->getSubSpaceCount();
00194           ompl::base::RealVectorStateSpace *subspace = new ompl::base::RealVectorStateSpace(0);
00195           subspace->setName("real_vector");
00196           state_space->addSubSpace(ompl::base::StateSpacePtr(subspace),1.0);
00197         }
00198         ompl::base::StateSpacePtr real_vector_state_space = state_space->getSubSpace("real_vector");
00199         double min_value, max_value;
00200         std::pair<double,double> bounds = revolute_joint->getVariableBounds(joint_name);
00201         min_value = bounds.first;
00202         max_value = bounds.second;
00203         real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->addDimension(joint_name,min_value,max_value);    
00204       }
00205     }    
00206   }
00207   return true;
00208 };
00209 
00210 
00211 ompl_ros_interface::MAPPING_TYPE getMappingType(const planning_models::KinematicModel::JointModel *joint_model)
00212 {
00213         const planning_models::KinematicModel::RevoluteJointModel* revolute_joint = 
00214     dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(joint_model);
00215   if(revolute_joint && revolute_joint->continuous_)
00216     return ompl_ros_interface::SO2;
00217   else if(revolute_joint)
00218     return ompl_ros_interface::REAL_VECTOR;
00219 
00220   const planning_models::KinematicModel::PlanarJointModel* planar_joint = 
00221     dynamic_cast<const planning_models::KinematicModel::PlanarJointModel*>(joint_model);
00222   if(planar_joint)
00223     return ompl_ros_interface::SE2;
00224 
00225   const planning_models::KinematicModel::FloatingJointModel* floating_joint = 
00226     dynamic_cast<const planning_models::KinematicModel::FloatingJointModel*>(joint_model);
00227   if(floating_joint)
00228     return ompl_ros_interface::SE3;
00229 
00230   return ompl_ros_interface::UNKNOWN;
00231 };
00232 
00233 ompl_ros_interface::MAPPING_TYPE getMappingType(const ompl::base::StateSpace *state_space)
00234 {
00235         const ompl::base::SO2StateSpace* SO2_state_space = 
00236     dynamic_cast<const ompl::base::SO2StateSpace*>(state_space);
00237   if(SO2_state_space)
00238     return ompl_ros_interface::SO2;
00239 
00240         const ompl::base::SE2StateSpace* SE2_state_space = 
00241     dynamic_cast<const ompl::base::SE2StateSpace*>(state_space);
00242   if(SE2_state_space)
00243     return ompl_ros_interface::SE2;
00244 
00245         const ompl::base::SO3StateSpace* SO3_state_space = 
00246     dynamic_cast<const ompl::base::SO3StateSpace*>(state_space);
00247   if(SO3_state_space)
00248     return ompl_ros_interface::SO3;
00249 
00250         const ompl::base::SE3StateSpace* SE3_state_space = 
00251     dynamic_cast<const ompl::base::SE3StateSpace*>(state_space);
00252   if(SE3_state_space)
00253     return ompl_ros_interface::SE3;
00254 
00255         const ompl::base::RealVectorStateSpace* real_vector_state_space = 
00256     dynamic_cast<const ompl::base::RealVectorStateSpace*>(state_space);
00257   if(real_vector_state_space)
00258     return ompl_ros_interface::REAL_VECTOR;
00259 
00260   return ompl_ros_interface::UNKNOWN;
00261 };
00262 
00263 ompl_ros_interface::MAPPING_TYPE getMappingType(const ompl::base::StateSpacePtr &state_space)
00264 {
00265   return getMappingType((const ompl::base::StateSpace*) state_space.get());
00266 };
00267 
00268 
00269 bool getJointStateGroupToOmplStateMapping(planning_models::KinematicState::JointStateGroup* joint_state_group, 
00270                                           const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00271                                           ompl_ros_interface::KinematicStateToOmplStateMapping &mapping)
00272 {
00273   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00274   unsigned int num_states = joint_state_group->getDimension();
00275   mapping.joint_state_mapping.resize(num_states,-1);
00276   mapping.joint_mapping_type.resize(num_states,ompl_ros_interface::UNKNOWN);
00277 
00278   bool joint_state_index_found = false;
00279   for(unsigned int j=0; j < num_state_spaces; j++)
00280   {
00281     if(dynamic_cast<ompl::base::RealVectorStateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j).get()))
00282     {
00283       mapping.real_vector_index = j;
00284       joint_state_index_found = true;
00285       break;
00286     }
00287   }
00288   std::vector<std::string> joint_names = joint_state_group->getJointNames();
00289   const planning_models::KinematicModel::JointModelGroup* joint_model_group = joint_state_group->getJointModelGroup();
00290   std::vector<const planning_models::KinematicModel::JointModel*> joint_models = joint_model_group->getJointModels();
00291 
00292   for(unsigned int i=0; i < num_states; i++)
00293   {
00294     std::string name = joint_names[i];
00295     bool mapping_found = false;
00296     for(unsigned int j=0; j < num_state_spaces; j++)
00297     {
00298       if(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j)->getName() == name)
00299       {
00300         mapping.joint_state_mapping[i] = j;
00301         mapping.joint_mapping_type[i] = getMappingType(joint_models[i]);
00302         mapping_found = true;
00303         break;
00304       }    
00305     }
00306     if(!mapping_found)
00307     {
00308       //search through the real vector if it exists
00309       if(joint_state_index_found)
00310       {
00311         ompl::base::StateSpacePtr real_vector_state_space = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
00312         for(unsigned int j=0; j < real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(); j++)
00313         {
00314           if(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(j) == name)
00315           {
00316             mapping.joint_state_mapping[i] = j;
00317             mapping_found = true;
00318             mapping.joint_mapping_type[i] = ompl_ros_interface::REAL_VECTOR;
00319             break;
00320           } 
00321         }
00322       }        
00323     }
00324     if(!mapping_found)
00325     {
00326       ROS_ERROR("Could not find mapping for joint %s",name.c_str());
00327       return false;
00328     }
00329   }
00330   return true;
00331 };
00332 
00333 bool getOmplStateToJointStateGroupMapping(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00334                                           planning_models::KinematicState::JointStateGroup* joint_state_group,
00335                                           ompl_ros_interface::OmplStateToKinematicStateMapping &mapping)
00336 {
00337   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00338   unsigned int num_states = joint_state_group->getDimension();
00339   mapping.ompl_state_mapping.resize(num_state_spaces,-1);
00340   mapping.mapping_type.resize(num_state_spaces,ompl_ros_interface::UNKNOWN);
00341 
00342   bool joint_state_index_found = false;
00343   for(unsigned int j=0; j < num_state_spaces; j++)
00344   {
00345     if(dynamic_cast<ompl::base::RealVectorStateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j).get()))
00346     {
00347       mapping.real_vector_index = j;
00348       mapping.mapping_type[j] = ompl_ros_interface::REAL_VECTOR;
00349       joint_state_index_found = true;
00350       break;
00351     }
00352   }
00353 
00354   std::vector<std::string> joint_names = joint_state_group->getJointNames();
00355   std::vector<const planning_models::KinematicModel::JointModel*> joint_models = joint_state_group->getJointModelGroup()->getJointModels();
00356   for(unsigned int i=0; i < num_state_spaces; i++)
00357   {
00358     bool mapping_found = false;
00359     for(unsigned int j=0; j < num_states; j++)
00360     {
00361       if(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName() == joint_names[j])
00362       {
00363         mapping.ompl_state_mapping[i] = j;
00364         mapping.mapping_type[i] = getMappingType(joint_models[j]);
00365         mapping_found = true;
00366         break;
00367       }    
00368     }
00369   }
00370   ompl::base::StateSpacePtr real_vector_state_space = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
00371   mapping.real_vector_mapping.resize(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(),-1);
00372   for(unsigned int i=0; i < real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(); i++)
00373   {
00374     bool ompl_real_vector_mapping_found = false;
00375     for(unsigned int j=0; j < num_states; j++)
00376     {
00377       if(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(i) == joint_names[j])
00378       {
00379         mapping.real_vector_mapping[i] = j;
00380         ompl_real_vector_mapping_found = true;
00381       } 
00382     }
00383     if(!ompl_real_vector_mapping_found)
00384     {
00385       ROS_ERROR("Could not find mapping for joint_state %s",real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(i).c_str());
00386       return false;
00387     }
00388   }  
00389   return true;
00390 };
00391 
00392 bool getRobotStateToOmplStateMapping(const motion_planning_msgs::RobotState &robot_state, 
00393                                      const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00394                                      ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00395                                      const bool &fail_if_match_not_found)
00396 {
00397   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00398   mapping.multi_dof_mapping.resize(robot_state.multi_dof_joint_state.joint_names.size(),-1);
00399   mapping.multi_dof_joint_mapping_type.resize(robot_state.multi_dof_joint_state.joint_names.size(),ompl_ros_interface::UNKNOWN);
00400   for(unsigned int i=0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++)
00401   {
00402     std::string name = robot_state.multi_dof_joint_state.joint_names[i];
00403     bool mapping_found = false;
00404     for(unsigned int j=0; j < num_state_spaces; j++)
00405     {
00406       if(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j)->getName() == name)
00407       {
00408         mapping.multi_dof_mapping[i] = j;
00409         mapping.multi_dof_joint_mapping_type[i] = getMappingType((ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j)).get());
00410         mapping_found = true;
00411         break;
00412       }    
00413     }
00414     if(!mapping_found && fail_if_match_not_found)
00415     {
00416       ROS_ERROR("Could not find mapping for multi_dof_joint_state %s",name.c_str());
00417       return false;
00418     }
00419   }
00420   mapping.real_vector_index = -1;
00421   //If robot state has no joint state, don't care about this part
00422   if(robot_state.joint_state.name.empty())
00423     return true;
00424   else
00425     return getJointStateToOmplStateMapping(robot_state.joint_state,ompl_scoped_state,mapping,fail_if_match_not_found);
00426 };
00427 
00428 bool getJointStateToOmplStateMapping(const sensor_msgs::JointState &joint_state, 
00429                                      const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00430                                      ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00431                                      const bool &fail_if_match_not_found)
00432 {
00433   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00434   bool joint_state_index_found = false;
00435   for(unsigned int j=0; j < num_state_spaces; j++)
00436   {
00437     if(dynamic_cast<ompl::base::RealVectorStateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j).get()))
00438     {
00439       mapping.real_vector_index = j;
00440       joint_state_index_found = true;
00441       break;
00442     }
00443   }
00444   if(!joint_state_index_found && fail_if_match_not_found)
00445     return false;
00446   mapping.joint_state_mapping.resize(joint_state.name.size(),-1);  
00447   mapping.joint_mapping_type.resize(joint_state.name.size(),ompl_ros_interface::UNKNOWN);
00448   ompl::base::StateSpacePtr real_vector_state_space = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
00449   for(unsigned int i=0; i < joint_state.name.size(); i++)
00450   {
00451     bool joint_state_mapping_found = false;
00452     for(unsigned int j=0; j < real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(); j++)
00453     {
00454       if(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(j) == joint_state.name[i])
00455       {
00456         mapping.joint_state_mapping[i] = j;
00457         joint_state_mapping_found = true;
00458         mapping.joint_mapping_type[i] = ompl_ros_interface::REAL_VECTOR;
00459       } 
00460     }
00461     for(unsigned int j=0; j < num_state_spaces; j++)
00462     {
00463       if(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j)->getName() == joint_state.name[i])
00464       {
00465         mapping.joint_state_mapping[i] = j;
00466         joint_state_mapping_found = true;
00467         mapping.joint_mapping_type[i] = ompl_ros_interface::SO2;
00468         break;
00469       }
00470     }
00471     if(!joint_state_mapping_found && fail_if_match_not_found)
00472     {
00473       ROS_ERROR("Could not find mapping for joint_state %s",joint_state.name[i].c_str());
00474       return false;
00475     }
00476   }  
00477   return true;
00478 };
00479 
00480 bool getOmplStateToJointStateMapping(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00481                                      const sensor_msgs::JointState &joint_state,
00482                                      ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00483                                      const bool &fail_if_match_not_found)
00484 {
00485   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00486   if(mapping.ompl_state_mapping.size() != num_state_spaces)
00487     mapping.ompl_state_mapping.resize(num_state_spaces,-1);
00488   if(mapping.mapping_type.size() != num_state_spaces)
00489     mapping.mapping_type.resize(num_state_spaces,ompl_ros_interface::UNKNOWN);
00490   bool ompl_real_vector_index_found = false;
00491   for(unsigned int j=0; j < num_state_spaces; j++)
00492   {
00493     if(dynamic_cast<ompl::base::RealVectorStateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(j).get()))
00494     {
00495       mapping.real_vector_index = j;
00496       mapping.mapping_type[j] = ompl_ros_interface::REAL_VECTOR;
00497       ompl_real_vector_index_found = true;
00498       break;
00499     }
00500   }
00501   if(!ompl_real_vector_index_found)
00502     return false;
00503 
00504   ompl::base::StateSpacePtr real_vector_state_space = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
00505   mapping.real_vector_mapping.resize(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(),-1);
00506   for(unsigned int i=0; i < real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(); i++)
00507   {
00508     bool ompl_real_vector_mapping_found = false;
00509     for(unsigned int j=0; j < joint_state.name.size(); j++)
00510     {
00511       if(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(i) == joint_state.name[j])
00512       {
00513         mapping.real_vector_mapping[i] = j;
00514         ompl_real_vector_mapping_found = true;
00515       } 
00516     }
00517     if(fail_if_match_not_found && !ompl_real_vector_mapping_found)
00518     {
00519       ROS_ERROR("Could not find mapping for joint_state %s",real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(i).c_str());
00520       return false;
00521     }
00522   }  
00523   // Now do SO(2) separately
00524   for(unsigned int i=0; i < num_state_spaces; i++)
00525   {
00526     if(dynamic_cast<ompl::base::SO2StateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i).get()))
00527     {
00528       bool ompl_state_mapping_found = false;
00529       for(unsigned int j=0; j < joint_state.name.size(); j++)
00530       {
00531         if(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName() == joint_state.name[j])
00532         {
00533           mapping.ompl_state_mapping[i] = j;
00534           mapping.mapping_type[i] = ompl_ros_interface::SO2;
00535           ompl_state_mapping_found = true;
00536           break;
00537         } 
00538       }
00539       if(fail_if_match_not_found && !ompl_state_mapping_found)
00540       {
00541         ROS_ERROR("Could not find mapping for ompl state %s",ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName().c_str());
00542         return false;
00543       }
00544     }
00545   }  
00546   return true;
00547 };
00548 
00549 bool getOmplStateToRobotStateMapping(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00550                                      const motion_planning_msgs::RobotState &robot_state, 
00551                                      ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00552                                      const bool &fail_if_match_not_found)
00553 {
00554   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00555   mapping.ompl_state_mapping.resize(num_state_spaces,-1);
00556   mapping.mapping_type.resize(num_state_spaces,ompl_ros_interface::UNKNOWN);
00557   for(unsigned int i=0; i < num_state_spaces; i++)
00558   {
00559     bool ompl_state_mapping_found = false;
00560     for(unsigned int j=0; j < robot_state.multi_dof_joint_state.joint_names.size(); j++)
00561     {
00562       if(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName() == robot_state.multi_dof_joint_state.joint_names[j])
00563       {
00564         mapping.ompl_state_mapping[i] = j;
00565         mapping.mapping_type[i] = getMappingType((ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)).get());
00566         ompl_state_mapping_found = true;
00567         break;
00568       } 
00569     }
00570     if(fail_if_match_not_found && !ompl_state_mapping_found && !dynamic_cast<ompl::base::SO2StateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i).get()) && !dynamic_cast<ompl::base::RealVectorStateSpace*>(ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i).get()))
00571     {
00572       ROS_ERROR("Could not find mapping for ompl state %s",ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName().c_str());
00573       return false;
00574     }
00575   }
00576   return getOmplStateToJointStateMapping(ompl_scoped_state,robot_state.joint_state,mapping,fail_if_match_not_found);
00577 };
00578 
00579 bool omplStateToRobotState(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00580                            const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00581                            motion_planning_msgs::RobotState &robot_state)
00582 {
00583   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00584   for(unsigned int i=0; i < num_state_spaces; i++)
00585   {
00586     if(mapping.mapping_type[i] == ompl_ros_interface::SO2 && mapping.ompl_state_mapping[i] > -1)
00587       robot_state.joint_state.position[mapping.ompl_state_mapping[i]] = ompl_scoped_state->as<ompl::base::SO2StateSpace::StateType>(i)->value;
00588     else if(mapping.mapping_type[i] == ompl_ros_interface::SE3 && mapping.ompl_state_mapping[i] > -1)
00589       ompl_ros_interface::SE3StateSpaceToPoseMsg(*(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)),robot_state.multi_dof_joint_state.poses[mapping.ompl_state_mapping[i]]);
00590     else if(mapping.mapping_type[i] == ompl_ros_interface::REAL_VECTOR) // real vector value
00591     {
00592       //      ompl::base::StateSpacePtr real_vector_state_space = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
00593       ompl_ros_interface::omplRealVectorStateToJointState(*(ompl_scoped_state->as<ompl::base::RealVectorStateSpace::StateType>(i)),mapping,robot_state.joint_state);
00594     }
00595   }
00596   return true;
00597 };
00598 
00599 bool omplRealVectorStateToJointState(const ompl::base::RealVectorStateSpace::StateType &ompl_state,
00600                                      const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00601                                      sensor_msgs::JointState &joint_state)
00602 {
00603   for(unsigned int i=0; i < mapping.real_vector_mapping.size(); i++)
00604     if(mapping.real_vector_mapping[i] > -1)
00605       joint_state.position[mapping.real_vector_mapping[i]] = ompl_state.values[i];
00606   return true;
00607 };
00608 
00609 
00610 bool robotStateToOmplState(const motion_planning_msgs::RobotState &robot_state,
00611                            const ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00612                            ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00613                            const bool &fail_if_match_not_found)
00614 {
00615   return robotStateToOmplState(robot_state,mapping,ompl_scoped_state.get(),fail_if_match_not_found);
00616 };
00617 
00618 bool robotStateToOmplState(const motion_planning_msgs::RobotState &robot_state,
00619                            const ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00620                            ompl::base::State *ompl_state,
00621                            const bool &fail_if_match_not_found)
00622 {
00623   for(unsigned int i=0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++)
00624   {
00625     if(mapping.multi_dof_mapping[i] > -1)
00626       ompl_ros_interface::poseMsgToSE3StateSpace(robot_state.multi_dof_joint_state.poses[i],
00627                                                *(ompl_state->as<ompl::base::CompoundState>()->as<ompl::base::SE3StateSpace::StateType>(mapping.multi_dof_mapping[i])));
00628   }
00629   if(mapping.real_vector_index > -1)
00630   {
00631     for(unsigned int i=0; i < robot_state.joint_state.name.size(); i++)
00632     {
00633       if(mapping.joint_mapping_type[i] == ompl_ros_interface::SO2 && mapping.joint_state_mapping[i] > -1)
00634         ompl_state->as<ompl::base::CompoundState>()->as<ompl::base::SO2StateSpace::StateType>(mapping.joint_state_mapping[i])->value = angles::normalize_angle(robot_state.joint_state.position[i]);
00635     }
00636   }
00637   if(mapping.real_vector_index > -1)
00638   {
00639     return jointStateToRealVectorState(robot_state.joint_state,
00640                                        mapping,
00641                                        *(ompl_state->as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(mapping.real_vector_index)),
00642                                        fail_if_match_not_found);
00643   }
00644   return true;                                       
00645 };
00646 
00647 bool robotStateToOmplState(const motion_planning_msgs::RobotState &robot_state,
00648                            ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00649                            const bool &fail_if_match_not_found)
00650 {
00651   ompl_ros_interface::RobotStateToOmplStateMapping mapping;
00652   if(!getRobotStateToOmplStateMapping(robot_state,ompl_scoped_state,mapping,fail_if_match_not_found))
00653     return false;
00654   return robotStateToOmplState(robot_state,mapping,ompl_scoped_state.get(),fail_if_match_not_found);
00655 };
00656 
00657 bool jointStateToRealVectorState(const sensor_msgs::JointState &joint_state,
00658                                  const ompl_ros_interface::RobotStateToOmplStateMapping &mapping,
00659                                  ompl::base::RealVectorStateSpace::StateType &real_vector_state,
00660                                  const bool &fail_if_match_not_found)
00661 {
00662   for(unsigned int i=0; i < joint_state.name.size(); i++)
00663     if(mapping.joint_mapping_type[i] == ompl_ros_interface::REAL_VECTOR && mapping.joint_state_mapping[i] > -1)
00664       real_vector_state.as<ompl::base::RealVectorStateSpace::StateType>()->values[mapping.joint_state_mapping[i]] = joint_state.position[i];          
00665   return true;
00666 };
00667 
00668 void SE3StateSpaceToPoseMsg(const ompl::base::SE3StateSpace::StateType &pose,
00669                           geometry_msgs::Pose &pose_msg)
00670 {
00671   pose_msg.position.x = pose.getX();
00672   pose_msg.position.y = pose.getY();
00673   pose_msg.position.z = pose.getZ();
00674   ompl::base::SO3StateSpace::StateType quaternion;
00675   quaternion.x = pose.rotation().x;
00676   quaternion.y = pose.rotation().y;
00677   quaternion.z = pose.rotation().z;
00678   quaternion.w = pose.rotation().w;
00679 
00680   SO3StateSpaceToQuaternionMsg(quaternion,pose_msg.orientation);
00681 };
00682 
00683 void SO3StateSpaceToPoseMsg(const ompl::base::SO3StateSpace::StateType &quaternion,
00684                           geometry_msgs::Pose &pose_msg)
00685 {
00686   SO3StateSpaceToQuaternionMsg(quaternion,pose_msg.orientation);
00687 };
00688 
00689 void SE2StateSpaceToPoseMsg(const ompl::base::SE2StateSpace::StateType &pose,
00690                           geometry_msgs::Pose &pose_msg)
00691 {
00692   pose_msg.position.x = pose.getX();
00693   pose_msg.position.y = pose.getY();
00694   //TODO set theta
00695 };
00696 
00697 
00698 void SO3StateSpaceToQuaternionMsg(const ompl::base::SO3StateSpace::StateType &quaternion,
00699                                 geometry_msgs::Quaternion &quaternion_msg)
00700 {
00701   quaternion_msg.x = quaternion.x;
00702   quaternion_msg.y = quaternion.y;
00703   quaternion_msg.z = quaternion.z;
00704   quaternion_msg.w = quaternion.w;
00705 };
00706 
00707 void poseMsgToSE3StateSpace(const geometry_msgs::Pose &pose_msg,
00708                           ompl::base::SE3StateSpace::StateType &pose)
00709 {
00710   pose.setX(pose_msg.position.x);
00711   pose.setY(pose_msg.position.y);
00712   pose.setZ(pose_msg.position.z);
00713   quaternionMsgToSO3StateSpace(pose_msg.orientation,pose.rotation());  
00714 };
00715 
00716 void quaternionMsgToSO3StateSpace(const geometry_msgs::Quaternion &quaternion_msg,
00717                                 ompl::base::SO3StateSpace::StateType &quaternion)
00718 {
00719   quaternion.x = quaternion_msg.x;
00720   quaternion.y = quaternion_msg.y;
00721   quaternion.z = quaternion_msg.z;
00722   quaternion.w = quaternion_msg.w;
00723 };
00724 
00731 bool kinematicStateGroupToOmplState(const planning_models::KinematicState::JointStateGroup* joint_state_group, 
00732                                     const ompl_ros_interface::KinematicStateToOmplStateMapping &mapping,
00733                                     ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state)
00734 {
00735   //  unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00736   unsigned int num_states = joint_state_group->getDimension();
00737 
00738   std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group->getJointStateVector();
00739   std::vector<std::string> joint_names = joint_state_group->getJointNames();
00740   for(unsigned int i=0; i < num_states; i++)
00741   {
00742     if(mapping.joint_mapping_type[i] == ompl_ros_interface::SO2)
00743     {
00744       std::vector<double> value = joint_states[i]->getJointStateValues();
00745       ompl_scoped_state->as<ompl::base::SO2StateSpace::StateType>(mapping.joint_state_mapping[i])->value = angles::normalize_angle(value[0]);
00746     }
00747     else if(mapping.joint_mapping_type[i] == ompl_ros_interface::SE2)
00748     {
00749       std::vector<double> value = joint_states[i]->getJointStateValues();
00750       ompl_scoped_state->as<ompl::base::SE2StateSpace::StateType>(mapping.joint_state_mapping[i])->setX(value[0]);
00751       ompl_scoped_state->as<ompl::base::SE2StateSpace::StateType>(mapping.joint_state_mapping[i])->setY(value[1]);
00752       ompl_scoped_state->as<ompl::base::SE2StateSpace::StateType>(mapping.joint_state_mapping[i])->setYaw(value[2]);
00753     }
00754     else if(mapping.joint_mapping_type[i] == ompl_ros_interface::SE3)
00755     {
00756       std::vector<double> value = joint_states[i]->getJointStateValues();
00757       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->setX(value[0]);
00758       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->setY(value[1]);
00759       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->setZ(value[2]);
00760       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->rotation().x = value[3];
00761       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->rotation().y = value[4];
00762       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->rotation().z = value[5];
00763       ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(mapping.joint_state_mapping[i])->rotation().w = value[6];
00764     }
00765     else if(mapping.joint_mapping_type[i] == ompl_ros_interface::REAL_VECTOR)
00766     {
00767       std::vector<double> value = joint_states[i]->getJointStateValues();
00768       ompl_scoped_state->as<ompl::base::RealVectorStateSpace::StateType>(mapping.real_vector_index)->values[mapping.joint_state_mapping[i]] = value[0];
00769     }
00770   }
00771   return true;
00772 };
00773 
00780 bool omplStateToKinematicStateGroup(const ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
00781                                     const ompl_ros_interface::OmplStateToKinematicStateMapping &mapping,
00782                                     planning_models::KinematicState::JointStateGroup* joint_state_group)
00783 {
00784   unsigned int num_state_spaces = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
00785   //  unsigned int num_states = joint_state_group->getDimension();
00786 
00787   std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group->getJointStateVector();
00788   for(unsigned int i=0; i < num_state_spaces; i++)
00789   {
00790     if(mapping.mapping_type[i] == ompl_ros_interface::SO2)
00791     {
00792       std::vector<double> value;
00793       value.push_back(ompl_scoped_state->as<ompl::base::SO2StateSpace::StateType>(i)->value);
00794       joint_states[mapping.ompl_state_mapping[i]]->setJointStateValues(value);
00795     }
00796     else if(mapping.mapping_type[i] == ompl_ros_interface::SE2)
00797     {
00798       std::vector<double> values;
00799       values.push_back(ompl_scoped_state->as<ompl::base::SE2StateSpace::StateType>(i)->getX());
00800       values.push_back(ompl_scoped_state->as<ompl::base::SE2StateSpace::StateType>(i)->getY());
00801       values.push_back(ompl_scoped_state->as<ompl::base::SE2StateSpace::StateType>(i)->getYaw());
00802       joint_states[mapping.ompl_state_mapping[i]]->setJointStateValues(values);
00803     }
00804     else if(mapping.mapping_type[i] == ompl_ros_interface::SE3)
00805     {
00806       std::vector<double> values;
00807       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->getX());
00808       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->getY());
00809       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->getZ());
00810       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().x);
00811       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().y);
00812       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().z);
00813       values.push_back(ompl_scoped_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().w);
00814       joint_states[mapping.ompl_state_mapping[i]]->setJointStateValues(values);
00815     }
00816     else if(mapping.mapping_type[i] == ompl_ros_interface::REAL_VECTOR)
00817     {
00818       ompl::base::StateSpacePtr real_vector_state_space = ompl_scoped_state.getSpace()->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
00819       unsigned int real_vector_size = mapping.real_vector_mapping.size();
00820       for(unsigned int j=0; j < real_vector_size; j++)
00821       {
00822         std::vector<double> values;
00823         values.push_back(ompl_scoped_state->as<ompl::base::RealVectorStateSpace::StateType>(mapping.real_vector_index)->values[j]);
00824         joint_states[mapping.real_vector_mapping[j]]->setJointStateValues(values);
00825       }
00826     }
00827   }
00828   return true;
00829 };
00830 
00837 bool omplStateToKinematicStateGroup(const ompl::base::State *ompl_state,
00838                                     const ompl_ros_interface::OmplStateToKinematicStateMapping &mapping,
00839                                     planning_models::KinematicState::JointStateGroup* joint_state_group)
00840 {
00841   unsigned int num_state_spaces = mapping.ompl_state_mapping.size();
00842   const ompl::base::CompoundState *ompl_compound_state = dynamic_cast<const ompl::base::CompoundState*> (ompl_state);
00843 
00844   std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group->getJointStateVector();
00845   for(unsigned int i=0; i < num_state_spaces; i++)
00846   {
00847     if(mapping.mapping_type[i] == ompl_ros_interface::SO2)
00848     {
00849       std::vector<double> value;
00850       value.push_back(ompl_compound_state->as<ompl::base::SO2StateSpace::StateType>(i)->value);
00851       joint_states[mapping.ompl_state_mapping[i]]->setJointStateValues(value);
00852     }
00853     else if(mapping.mapping_type[i] == ompl_ros_interface::SE2)
00854     {
00855       std::vector<double> values;
00856       values.push_back(ompl_compound_state->as<ompl::base::SE2StateSpace::StateType>(i)->getX());
00857       values.push_back(ompl_compound_state->as<ompl::base::SE2StateSpace::StateType>(i)->getY());
00858       values.push_back(ompl_compound_state->as<ompl::base::SE2StateSpace::StateType>(i)->getYaw());
00859       joint_states[mapping.ompl_state_mapping[i]]->setJointStateValues(values);
00860     }
00861     else if(mapping.mapping_type[i] == ompl_ros_interface::SE3)
00862     {
00863       std::vector<double> values;
00864       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->getX());
00865       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->getY());
00866       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->getZ());
00867       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().x);
00868       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().y);
00869       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().z);
00870       values.push_back(ompl_compound_state->as<ompl::base::SE3StateSpace::StateType>(i)->rotation().w);
00871       joint_states[mapping.ompl_state_mapping[i]]->setJointStateValues(values);
00872     }
00873     else if(mapping.mapping_type[i] == ompl_ros_interface::REAL_VECTOR)
00874     {
00875       unsigned int real_vector_size = mapping.real_vector_mapping.size();
00876       for(unsigned int j=0; j < real_vector_size; j++)
00877       {
00878         std::vector<double> values;
00879         values.push_back(ompl_compound_state->as<ompl::base::RealVectorStateSpace::StateType>(mapping.real_vector_index)->values[j]);
00880         joint_states[mapping.real_vector_mapping[j]]->setJointStateValues(values);
00881       }
00882     }
00883   }
00884   return true;
00885 };
00886 
00892 bool jointStateGroupToRobotTrajectory(planning_models::KinematicState::JointStateGroup* joint_state_group, 
00893                                       motion_planning_msgs::RobotTrajectory &robot_trajectory)
00894 {
00895   const planning_models::KinematicModel::JointModelGroup* joint_model_group = joint_state_group->getJointModelGroup();
00896   std::vector<const planning_models::KinematicModel::JointModel*> joint_models = joint_model_group->getJointModels();
00897   for(unsigned int i=0; i < joint_models.size(); i++)
00898   {
00899                 const planning_models::KinematicModel::RevoluteJointModel* revolute_joint = 
00900                         dynamic_cast<const planning_models::KinematicModel::RevoluteJointModel*>(joint_models[i]);
00901                 const planning_models::KinematicModel::PrismaticJointModel* prismatic_joint = 
00902                         dynamic_cast<const planning_models::KinematicModel::PrismaticJointModel*>(joint_models[i]);
00903     const planning_models::KinematicModel::PlanarJointModel* planar_joint = 
00904       dynamic_cast<const planning_models::KinematicModel::PlanarJointModel*>(joint_models[i]);
00905     const planning_models::KinematicModel::FloatingJointModel* floating_joint = 
00906       dynamic_cast<const planning_models::KinematicModel::FloatingJointModel*>(joint_models[i]);
00907     if (revolute_joint || prismatic_joint)
00908       robot_trajectory.joint_trajectory.joint_names.push_back(joint_models[i]->getName());
00909     else if (planar_joint || floating_joint)
00910     {
00911       robot_trajectory.multi_dof_joint_trajectory.joint_names.push_back(joint_models[i]->getName());
00912       robot_trajectory.multi_dof_joint_trajectory.frame_ids.push_back(joint_models[i]->getParentFrameId());
00913       robot_trajectory.multi_dof_joint_trajectory.child_frame_ids.push_back(joint_models[i]->getChildFrameId());
00914     }
00915     else
00916       return false;  
00917   }
00918   return true;
00919 };
00920 
00921 
00922 bool omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path,
00923                                         const ompl::base::StateSpacePtr &state_space,
00924                                         motion_planning_msgs::RobotTrajectory &robot_trajectory)
00925 {
00926   if(robot_trajectory.joint_trajectory.joint_names.empty() && robot_trajectory.multi_dof_joint_trajectory.joint_names.empty())
00927   {
00928     ROS_ERROR("Robot trajectory needs to initialized before calling this function");
00929     return false;
00930   }
00931   ompl_ros_interface::OmplStateToRobotStateMapping mapping;
00932   if(!getOmplStateToRobotTrajectoryMapping(state_space,robot_trajectory,mapping))
00933     return false;
00934   if(!omplPathGeometricToRobotTrajectory(path,mapping,robot_trajectory))
00935     return false;
00936   return true;
00937 };
00938 
00939 bool omplPathGeometricToRobotTrajectory(const ompl::geometric::PathGeometric &path,
00940                                         const ompl_ros_interface::OmplStateToRobotStateMapping &mapping,
00941                                         motion_planning_msgs::RobotTrajectory &robot_trajectory)
00942 {
00943   if(robot_trajectory.joint_trajectory.joint_names.empty() && robot_trajectory.multi_dof_joint_trajectory.joint_names.empty())
00944   {
00945     ROS_ERROR("Robot trajectory needs to initialized before calling this function");
00946     return false;
00947   }
00948   unsigned int num_points = path.states.size();
00949   unsigned int num_state_spaces = mapping.ompl_state_mapping.size();
00950   bool multi_dof = false;
00951   bool single_dof = false;
00952 
00953   if(!robot_trajectory.multi_dof_joint_trajectory.joint_names.empty())
00954   {
00955     multi_dof = true;
00956     robot_trajectory.multi_dof_joint_trajectory.points.resize(num_points);
00957     for(unsigned int i=0; i < num_points; i++)
00958       {
00959         robot_trajectory.multi_dof_joint_trajectory.points[i].poses.resize(robot_trajectory.multi_dof_joint_trajectory.joint_names.size());
00960         robot_trajectory.multi_dof_joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
00961       }
00962   }
00963   if(!robot_trajectory.joint_trajectory.joint_names.empty())
00964   {
00965     single_dof = true;
00966     robot_trajectory.joint_trajectory.points.resize(num_points);
00967     for(unsigned int i=0; i < num_points; i++)
00968       {      
00969         robot_trajectory.joint_trajectory.points[i].positions.resize(robot_trajectory.joint_trajectory.joint_names.size());
00970         robot_trajectory.joint_trajectory.points[i].time_from_start = ros::Duration(0.0);
00971       }
00972   }
00973   
00974   for(unsigned int i=0; i < num_points; i++)
00975   {
00976     for(unsigned int j=0; j < num_state_spaces; j++)
00977     {
00978       if(mapping.mapping_type[j] == ompl_ros_interface::SO2)
00979         robot_trajectory.joint_trajectory.points[i].positions[mapping.ompl_state_mapping[j]] = path.states[i]->as<ompl::base::CompoundState>()->as<ompl::base::SO2StateSpace::StateType>(j)->value;
00980       else if(mapping.mapping_type[j] == ompl_ros_interface::SE2)
00981         ompl_ros_interface::SE2StateSpaceToPoseMsg(*(path.states[i]->as<ompl::base::CompoundState>()->as<ompl::base::SE2StateSpace::StateType>(j)),robot_trajectory.multi_dof_joint_trajectory.points[i].poses[mapping.ompl_state_mapping[j]]);
00982       else if(mapping.mapping_type[j] == ompl_ros_interface::SE3)
00983         ompl_ros_interface::SE3StateSpaceToPoseMsg(*(path.states[i]->as<ompl::base::CompoundState>()->as<ompl::base::SE3StateSpace::StateType>(j)),robot_trajectory.multi_dof_joint_trajectory.points[i].poses[mapping.ompl_state_mapping[j]]);
00984       else if(mapping.mapping_type[j] == ompl_ros_interface::SO3)
00985         ompl_ros_interface::SO3StateSpaceToPoseMsg(*(path.states[i]->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(j)),robot_trajectory.multi_dof_joint_trajectory.points[i].poses[mapping.ompl_state_mapping[j]]);
00986       else // real vector value
00987       {
00988         ompl::base::State* real_vector_state = path.states[i]->as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(mapping.real_vector_index);
00989         for(unsigned int k=0; k < mapping.real_vector_mapping.size(); k++)
00990           robot_trajectory.joint_trajectory.points[i].positions[mapping.real_vector_mapping[k]] = real_vector_state->as<ompl::base::RealVectorStateSpace::StateType>()->values[k];
00991       }
00992     }
00993   }
00994   return true;  
00995 };
00996 
00997 bool getOmplStateToRobotTrajectoryMapping(const ompl::base::StateSpacePtr &state_space,
00998                                           const motion_planning_msgs::RobotTrajectory &robot_trajectory, 
00999                                           ompl_ros_interface::OmplStateToRobotStateMapping &mapping)
01000 {
01001   unsigned int num_state_spaces = state_space->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
01002   mapping.ompl_state_mapping.resize(num_state_spaces,-1);
01003   mapping.mapping_type.resize(num_state_spaces,ompl_ros_interface::UNKNOWN);
01004   for(unsigned int i=0; i < num_state_spaces; i++)
01005   {
01006     ROS_DEBUG("Trajectory state space %d has name %s",i,state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName().c_str());
01007     bool ompl_state_mapping_found = false;
01008     for(unsigned int j=0; j < robot_trajectory.multi_dof_joint_trajectory.joint_names.size(); j++)
01009     {
01010       if(state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName() == robot_trajectory.multi_dof_joint_trajectory.joint_names[j])
01011       {
01012         mapping.ompl_state_mapping[i] = j;
01013         mapping.mapping_type[i] = getMappingType((state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)).get());
01014         ompl_state_mapping_found = true;
01015         break;
01016       } 
01017     }
01018   }
01019   bool result = getOmplStateToJointTrajectoryMapping(state_space,robot_trajectory.joint_trajectory,mapping);
01020   for(int i=0; i < (int) num_state_spaces; i++)
01021   {
01022     if(mapping.ompl_state_mapping[i] < 0 && i != (int) mapping.real_vector_index)
01023     {
01024       ROS_ERROR("Could not find mapping for state space %s",state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName().c_str());
01025       return false;
01026     }
01027   };
01028   return result;
01029 };
01030 
01031 bool getOmplStateToJointTrajectoryMapping(const ompl::base::StateSpacePtr &state_space,
01032                                           const trajectory_msgs::JointTrajectory &joint_trajectory,
01033                                           ompl_ros_interface::OmplStateToRobotStateMapping &mapping)
01034 {
01035   unsigned int num_state_spaces = state_space->as<ompl::base::CompoundStateSpace>()->getSubSpaceCount();
01036   if(mapping.ompl_state_mapping.size() != num_state_spaces)
01037     mapping.ompl_state_mapping.resize(num_state_spaces,-1);
01038   if(mapping.mapping_type.size() != num_state_spaces)
01039     mapping.mapping_type.resize(num_state_spaces,ompl_ros_interface::UNKNOWN);
01040 
01041   // Do SO(2) separately
01042   for(unsigned int i=0; i < num_state_spaces; i++)
01043   {
01044     if(dynamic_cast<ompl::base::SO2StateSpace*>(state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i).get()))
01045     {
01046       bool ompl_state_mapping_found = false;
01047       for(unsigned int j=0; j < joint_trajectory.joint_names.size(); j++)
01048       {
01049         if(state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName() == joint_trajectory.joint_names[j])
01050         {
01051           mapping.ompl_state_mapping[i] = j;
01052           mapping.mapping_type[i] = ompl_ros_interface::SO2;
01053           ompl_state_mapping_found = true;
01054           ROS_DEBUG("Trajectory mapping for SO2 joint %s from %d to %d",joint_trajectory.joint_names[j].c_str(),i,j);
01055           break;
01056         } 
01057       }
01058       if(!ompl_state_mapping_found)
01059       {
01060         ROS_ERROR("Could not find mapping for ompl state %s",state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(i)->getName().c_str());
01061         return false;
01062       }
01063     }
01064   }  
01065 
01066   bool ompl_real_vector_index_found = false;
01067   for(unsigned int j=0; j < num_state_spaces; j++)
01068   {
01069     if(dynamic_cast<ompl::base::RealVectorStateSpace*>(state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(j).get()))
01070     {
01071       mapping.real_vector_index = j;
01072       mapping.mapping_type[j] = ompl_ros_interface::REAL_VECTOR;
01073       ompl_real_vector_index_found = true;
01074       break;
01075     }
01076   }
01077   if(!ompl_real_vector_index_found)
01078   {
01079     ROS_WARN("No real vector state space in ompl state space");
01080     return true;
01081   }
01082 
01083   ompl::base::StateSpacePtr real_vector_state_space = state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping.real_vector_index);
01084   mapping.real_vector_mapping.resize(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(),-1);
01085   for(unsigned int i=0; i < real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimension(); i++)
01086   {
01087     bool ompl_real_vector_mapping_found = false;
01088     for(unsigned int j=0; j < joint_trajectory.joint_names.size(); j++)
01089     {
01090       if(real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(i) == joint_trajectory.joint_names[j])
01091       {
01092         mapping.real_vector_mapping[i] = j;
01093         ompl_real_vector_mapping_found = true;
01094         ROS_DEBUG("Trajectory mapping for revolute joint %s from %d to %d",joint_trajectory.joint_names[j].c_str(),i,j);
01095       } 
01096     }
01097     if(!ompl_real_vector_mapping_found)
01098     {
01099       ROS_ERROR("Could not find mapping for ompl state %s",real_vector_state_space->as<ompl::base::RealVectorStateSpace>()->getDimensionName(i).c_str());
01100       return false;
01101     }
01102   }  
01103   return true;
01104 };
01105 
01106 bool jointConstraintsToOmplState(const std::vector<motion_planning_msgs::JointConstraint> &joint_constraints,
01107                                  ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state)
01108 {
01109   sensor_msgs::JointState joint_state = motion_planning_msgs::jointConstraintsToJointState(joint_constraints);
01110   for(unsigned int i=0; i < joint_state.name.size(); i++)
01111   {
01112     ROS_DEBUG("Joint %s: %f",joint_state.name[i].c_str(), joint_state.position[i]);
01113   }
01114   motion_planning_msgs::RobotState robot_state;
01115   robot_state.joint_state = joint_state;
01116 
01117   ompl_ros_interface::RobotStateToOmplStateMapping mapping;
01118   if(ompl_ros_interface::getJointStateToOmplStateMapping(joint_state,ompl_scoped_state,mapping))
01119     return ompl_ros_interface::robotStateToOmplState(robot_state,mapping,ompl_scoped_state);
01120   return false;
01121 };
01122 
01123 bool constraintsToOmplState(const motion_planning_msgs::Constraints &constraints,
01124                             ompl::base::ScopedState<ompl::base::CompoundStateSpace> &ompl_scoped_state,
01125                             const bool &fail_if_match_not_found)
01126 {
01127   motion_planning_msgs::RobotState robot_state;
01128   robot_state.joint_state = motion_planning_msgs::jointConstraintsToJointState(constraints.joint_constraints);
01129   ROS_DEBUG("There are %lu joint constraints",constraints.joint_constraints.size());
01130   for(unsigned int i=0; i < robot_state.joint_state.name.size(); i++)
01131     ROS_DEBUG("Joint Constraint:: Joint %s: %f",robot_state.joint_state.name[i].c_str(), robot_state.joint_state.position[i]);
01132 
01133   robot_state.multi_dof_joint_state = motion_planning_msgs::poseConstraintsToMultiDOFJointState(constraints.position_constraints,
01134                                                                                                 constraints.orientation_constraints);
01135 
01136   ompl_ros_interface::RobotStateToOmplStateMapping mapping;
01137   if(ompl_ros_interface::getRobotStateToOmplStateMapping(robot_state,ompl_scoped_state,mapping,fail_if_match_not_found))
01138     return ompl_ros_interface::robotStateToOmplState(robot_state,mapping,ompl_scoped_state,fail_if_match_not_found);
01139   return false;
01140 };
01141 
01142 bool getRobotStateToJointModelGroupMapping(const motion_planning_msgs::RobotState &robot_state,
01143                                            const planning_models::KinematicModel::JointModelGroup *joint_model_group,
01144                                            ompl_ros_interface::RobotStateToKinematicStateMapping &mapping)
01145 {
01146   std::vector<const planning_models::KinematicModel::JointModel*> joint_models = joint_model_group->getJointModels();
01147   mapping.joint_state_mapping.resize(robot_state.joint_state.name.size(),-1);
01148   mapping.multi_dof_mapping.resize(robot_state.multi_dof_joint_state.joint_names.size(),-1);
01149 
01150   for (unsigned int i = 0 ; i < robot_state.joint_state.name.size(); ++i)
01151   {
01152     for(unsigned int j=0; j < joint_models.size(); j++)
01153     {
01154       if(robot_state.joint_state.name[i] == joint_models[j]->getName())
01155       {
01156         mapping.joint_state_mapping[i] = j;
01157         break;
01158       }
01159     }
01160     if(mapping.joint_state_mapping[i] < 0)
01161     {
01162       ROS_ERROR("Mapping does not exist for joint %s",robot_state.joint_state.name[i].c_str());
01163       return false;
01164     }
01165   }
01166   for (unsigned int i = 0 ; i < robot_state.multi_dof_joint_state.joint_names.size(); ++i)
01167   {
01168     for(unsigned int j=0; j < joint_models.size(); j++)
01169     {
01170       if(robot_state.multi_dof_joint_state.joint_names[i] == joint_models[j]->getName())
01171       {
01172         mapping.multi_dof_mapping[i] = j;
01173         break;
01174       }
01175     }
01176     if(mapping.multi_dof_mapping[i] < 0)
01177     {
01178       ROS_ERROR("Mapping does not exist for joint %s",robot_state.multi_dof_joint_state.joint_names[i].c_str());
01179       return false;
01180     }
01181   }
01182   return true;
01183 }
01184 
01185 bool robotStateToJointStateGroup(const motion_planning_msgs::RobotState &robot_state,
01186                                  const ompl_ros_interface::RobotStateToKinematicStateMapping &mapping,
01187                                  planning_models::KinematicState::JointStateGroup *joint_state_group)
01188 {
01189   std::vector<planning_models::KinematicState::JointState*> joint_states = joint_state_group->getJointStateVector();
01190   for(unsigned int i=0; i < robot_state.joint_state.name.size(); i++)
01191   {
01192     std::vector<double> value;
01193     value.push_back(robot_state.joint_state.position[i]);
01194     joint_states[mapping.joint_state_mapping[i]]->setJointStateValues(value);
01195   }    
01196 
01197   for(unsigned int i=0; i < robot_state.multi_dof_joint_state.joint_names.size(); i++)
01198   {
01199     std::vector<double> values;
01200     values.push_back(robot_state.multi_dof_joint_state.poses[i].position.x);
01201     values.push_back(robot_state.multi_dof_joint_state.poses[i].position.y);
01202     values.push_back(robot_state.multi_dof_joint_state.poses[i].position.z);
01203 
01204     values.push_back(robot_state.multi_dof_joint_state.poses[i].orientation.x);
01205     values.push_back(robot_state.multi_dof_joint_state.poses[i].orientation.y);
01206     values.push_back(robot_state.multi_dof_joint_state.poses[i].orientation.z);
01207     values.push_back(robot_state.multi_dof_joint_state.poses[i].orientation.w);
01208 
01209     int index = mapping.multi_dof_mapping[i];
01210     joint_states[index]->setJointStateValues(values);
01211   }
01212   return true;
01213 }                                 
01214 }