00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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_DEBUG("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_DEBUG("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 else
00107 {
00108 ROS_ERROR("Could not initialize projection evaluator. A projection evaluator needs to be defined as either a combination of revolute joints with joint limits, or a continuous, spherical, planar of floating joint. ");
00109 throw new OMPLROSException();
00110 }
00111 };
00112
00113 unsigned int OmplRosProjectionEvaluator::getDimension(void) const
00114 {
00115 return dimension_;
00116 };
00117
00118 void OmplRosProjectionEvaluator::project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
00119 {
00120 if(mapping_type_ == ompl_ros_interface::REAL_VECTOR)
00121 {
00122 for(unsigned int i=0; i < dimension_; i++)
00123 projection.values[i] = state->as<ompl::base::CompoundState>()->as<ompl::base::RealVectorStateSpace::StateType>(mapping_index_)->values[i];
00124 }
00125 if(mapping_type_ == ompl_ros_interface::SO2)
00126 {
00127 projection.values[0] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO2StateSpace::StateType>(mapping_index_)->value;
00128 }
00129 else if(mapping_type_ == ompl_ros_interface::SE2)
00130 {
00131 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));
00132 }
00133 else if(mapping_type_ == ompl_ros_interface::SO3)
00134 {
00135 projection.values[0] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(mapping_index_)->x;
00136 projection.values[1] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(mapping_index_)->y;
00137 projection.values[2] = state->as<ompl::base::CompoundState>()->as<ompl::base::SO3StateSpace::StateType>(mapping_index_)->z;
00138 }
00139 else if(mapping_type_ == ompl_ros_interface::SE3)
00140 {
00141 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));
00142 }
00143 };
00144
00145 }
