ompl_ros_interface: ompl_ros_projection

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 
00035 #include <ompl_ros_interface/ompl_ros_projection_evaluator.h>
00036 
00037 namespace ompl_ros_interface
00038 { 
00039  
00040 OmplRosProjectionEvaluator::OmplRosProjectionEvaluator(const ompl::base::StateSpace *state_space, 
00041                                                        const std::string &evaluator_name) : ompl::base::ProjectionEvaluator(state_space)
00042 {
00043   if(!state_space->as<ompl::base::CompoundStateSpace>()->hasSubSpace(evaluator_name) && 
00044      !(evaluator_name == "joint_state"))
00045   {
00046     ROS_ERROR("Evaluator name %s does not match any state space name",evaluator_name.c_str());
00047     return;
00048   }
00049 
00050   if(evaluator_name == "joint_state")
00051   {
00052     if(!state_space->as<ompl::base::CompoundStateSpace>()->hasSubSpace("real_vector"))
00053     {      
00054       ROS_ERROR("Could not find subspace for defining projection evaluator");
00055       throw new OMPLROSException();
00056     }
00057     mapping_index_ = state_space->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex("real_vector");
00058     dimension_ = std::min<unsigned int>(state_space->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(mapping_index_)->getDimension(),2);
00059     cellSizes_.resize(dimension_);
00060     const ompl::base::RealVectorBounds &b = state_space->as<ompl::base::CompoundStateSpace>()->as<ompl::base::RealVectorStateSpace>(mapping_index_)->getBounds();
00061     for(unsigned int i=0; i < dimension_; i++)
00062       cellSizes_[i] = (b.high[i] - b.low[i]) / 10.0;
00063     mapping_type_ = ompl_ros_interface::REAL_VECTOR;
00064     ROS_INFO("Choosing projection evaluator for real vector joints with dimension %d",dimension_);
00065     return;
00066   }
00067 
00068   mapping_index_ = state_space->as<ompl::base::CompoundStateSpace>()->getSubSpaceIndex(evaluator_name);
00069   mapping_type_ = ompl_ros_interface::getMappingType(state_space->as<ompl::base::CompoundStateSpace>()->getSubSpace(mapping_index_).get());
00070 
00071   if(mapping_type_ == ompl_ros_interface::SO2)
00072   {
00073     dimension_ = 1;
00074     cellSizes_.resize(1);
00075     cellSizes_[0] = boost::math::constants::pi<double>() / 10.0;
00076     ROS_INFO("Choosing projection evaluator for SO2 state space %s",evaluator_name.c_str());
00077   }
00078   else if(mapping_type_ == ompl_ros_interface::SE2)
00079   {
00080     dimension_ = 2;
00081     cellSizes_.resize(2);
00082     const ompl::base::RealVectorBounds &b = state_space->as<ompl::base::CompoundStateSpace>()->as<ompl::base::SE2StateSpace>(mapping_index_)->as<ompl::base::RealVectorStateSpace>(0)->getBounds();
00083     cellSizes_[0] = (b.high[0] - b.low[0]) / 10.0;
00084     cellSizes_[1] = (b.high[1] - b.low[1]) / 10.0;      
00085     ROS_INFO("Choosing projection evaluator for SE2 state space %s",evaluator_name.c_str());
00086   }
00087   else if(mapping_type_ == ompl_ros_interface::SO3)
00088   {
00089     dimension_ = 3;
00090     cellSizes_.resize(3);
00091     cellSizes_[0] = boost::math::constants::pi<double>() / 10.0;
00092     cellSizes_[1] = boost::math::constants::pi<double>() / 10.0;
00093     cellSizes_[2] = boost::math::constants::pi<double>() / 10.0;
00094     ROS_INFO("Choosing projection evaluator for SO3 state space %s",evaluator_name.c_str());
00095   }
00096   else if(mapping_type_ == ompl_ros_interface::SE3)
00097   {
00098     dimension_ = 3;
00099     cellSizes_.resize(3);
00100     const ompl::base::RealVectorBounds &b = state_space->as<ompl::base::CompoundStateSpace>()->as<ompl::base::SE3StateSpace>(mapping_index_)->as<ompl::base::RealVectorStateSpace>(0)->getBounds();
00101     cellSizes_[0] = (b.high[0] - b.low[0]) / 10.0;
00102     cellSizes_[1] = (b.high[1] - b.low[1]) / 10.0;
00103     cellSizes_[2] = (b.high[2] - b.low[2]) / 10.0;
00104     ROS_INFO("Choosing projection evaluator for SE3 state space %s",evaluator_name.c_str());
00105   }
00106 };
00107         
00108 unsigned int OmplRosProjectionEvaluator::getDimension(void) const
00109 {
00110   return dimension_;
00111 };
00112         
00113 void OmplRosProjectionEvaluator::project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
00114 {
00115   if(mapping_type_ == ompl_ros_interface::REAL_VECTOR)
00116   {
00117     for(unsigned int i=0; i < dimension_; i++)
00118       projection.values[i] = state->as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(mapping_index_)->values[i];
00119   }
00120   if(mapping_type_ == ompl_ros_interface::SO2)
00121   {
00122     projection.values[0] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO2StateSpace::StateType>(mapping_index_)->value;
00123   }
00124   else if(mapping_type_ == ompl_ros_interface::SE2)
00125   {
00126     memcpy(projection.values, state->as<ompl::base::CompoundState>()->as<ompl::base::SE2StateSpace::StateType>(mapping_index_)->as<ompl::base::RealVectorStateSpace::StateType>(0)->values, 2 * sizeof(double));
00127   }
00128   else if(mapping_type_ == ompl_ros_interface::SO3)
00129   {
00130     projection.values[0] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(mapping_index_)->x;
00131     projection.values[1] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(mapping_index_)->y;
00132     projection.values[2] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(mapping_index_)->z;
00133   }
00134   else if(mapping_type_ == ompl_ros_interface::SE3)
00135   {
00136     memcpy(projection.values, state->as<ompl::base::CompoundState>()->as<ompl::base::SE3StateSpace::StateType>(mapping_index_)->as<ompl::base::RealVectorStateSpace::StateType>(0)->values, 3 * sizeof(double));
00137   }
00138 };
00139 
00140 }
ompl_ros_projection_evaluator.cpp
