model_based_state_space.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 * Software License Agreement (BSD License)
00003 *
00004 *  Copyright (c) 2012, 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 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 /* Author: Ioan Sucan */
00036 
00037 #include <moveit/ompl_interface/parameterization/model_based_state_space.h>
00038 #include <boost/bind.hpp>
00039 
00040 ompl_interface::ModelBasedStateSpace::ModelBasedStateSpace(const ModelBasedStateSpaceSpecification& spec)
00041   : ompl::base::StateSpace(), spec_(spec)
00042 {
00043   // set the state space name
00044   setName(spec_.joint_model_group_->getName());
00045   variable_count_ = spec_.joint_model_group_->getVariableCount();
00046   state_values_size_ = variable_count_ * sizeof(double);
00047   joint_model_vector_ = spec_.joint_model_group_->getActiveJointModels();
00048 
00049   // make sure we have bounds for every joint stored within the spec (use default bounds if not specified)
00050   if (!spec_.joint_bounds_.empty() && spec_.joint_bounds_.size() != joint_model_vector_.size())
00051   {
00052     logError("Joint group '%s' has incorrect bounds specified. Using the default bounds instead.",
00053              spec_.joint_model_group_->getName().c_str());
00054     spec_.joint_bounds_.clear();
00055   }
00056 
00057   // copy the default joint bounds if needed
00058   if (spec_.joint_bounds_.empty())
00059     spec_.joint_bounds_ = spec_.joint_model_group_->getActiveJointModelsBounds();
00060 
00061   // new perform a deep copy of the bounds, in case we need to modify them
00062   joint_bounds_storage_.resize(spec_.joint_bounds_.size());
00063   for (std::size_t i = 0; i < joint_bounds_storage_.size(); ++i)
00064   {
00065     joint_bounds_storage_[i] = *spec_.joint_bounds_[i];
00066     spec_.joint_bounds_[i] = &joint_bounds_storage_[i];
00067   }
00068 
00069   // default settings
00070   setTagSnapToSegment(0.95);
00071 
00073   params_.declareParam<double>("tag_snap_to_segment", boost::bind(&ModelBasedStateSpace::setTagSnapToSegment, this, _1),
00074                                boost::bind(&ModelBasedStateSpace::getTagSnapToSegment, this));
00075 }
00076 
00077 ompl_interface::ModelBasedStateSpace::~ModelBasedStateSpace()
00078 {
00079 }
00080 
00081 double ompl_interface::ModelBasedStateSpace::getTagSnapToSegment() const
00082 {
00083   return tag_snap_to_segment_;
00084 }
00085 
00086 void ompl_interface::ModelBasedStateSpace::setTagSnapToSegment(double snap)
00087 {
00088   if (snap < 0.0 || snap > 1.0)
00089     logWarn("Snap to segment for tags is a ratio. It's value must be between 0.0 and 1.0. Value remains as previously "
00090             "set (%lf)",
00091             tag_snap_to_segment_);
00092   else
00093   {
00094     tag_snap_to_segment_ = snap;
00095     tag_snap_to_segment_complement_ = 1.0 - tag_snap_to_segment_;
00096   }
00097 }
00098 
00099 ompl::base::State* ompl_interface::ModelBasedStateSpace::allocState() const
00100 {
00101   StateType* state = new StateType();
00102   state->values = new double[variable_count_];
00103   return state;
00104 }
00105 
00106 void ompl_interface::ModelBasedStateSpace::freeState(ompl::base::State* state) const
00107 {
00108   delete[] state->as<StateType>()->values;
00109   delete state->as<StateType>();
00110 }
00111 
00112 void ompl_interface::ModelBasedStateSpace::copyState(ompl::base::State* destination,
00113                                                      const ompl::base::State* source) const
00114 {
00115   memcpy(destination->as<StateType>()->values, source->as<StateType>()->values, state_values_size_);
00116   destination->as<StateType>()->tag = source->as<StateType>()->tag;
00117   destination->as<StateType>()->flags = source->as<StateType>()->flags;
00118   destination->as<StateType>()->distance = source->as<StateType>()->distance;
00119 }
00120 
00121 unsigned int ompl_interface::ModelBasedStateSpace::getSerializationLength() const
00122 {
00123   return state_values_size_ + sizeof(int);
00124 }
00125 
00126 void ompl_interface::ModelBasedStateSpace::serialize(void* serialization, const ompl::base::State* state) const
00127 {
00128   *reinterpret_cast<int*>(serialization) = state->as<StateType>()->tag;
00129   memcpy(reinterpret_cast<char*>(serialization) + sizeof(int), state->as<StateType>()->values, state_values_size_);
00130 }
00131 
00132 void ompl_interface::ModelBasedStateSpace::deserialize(ompl::base::State* state, const void* serialization) const
00133 {
00134   state->as<StateType>()->tag = *reinterpret_cast<const int*>(serialization);
00135   memcpy(state->as<StateType>()->values, reinterpret_cast<const char*>(serialization) + sizeof(int),
00136          state_values_size_);
00137 }
00138 
00139 unsigned int ompl_interface::ModelBasedStateSpace::getDimension() const
00140 {
00141   unsigned int d = 0;
00142   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00143     d += joint_model_vector_[i]->getStateSpaceDimension();
00144   return d;
00145 }
00146 
00147 double ompl_interface::ModelBasedStateSpace::getMaximumExtent() const
00148 {
00149   return spec_.joint_model_group_->getMaximumExtent(spec_.joint_bounds_);
00150 }
00151 
00152 double ompl_interface::ModelBasedStateSpace::getMeasure() const
00153 {
00154   double m = 1.0;
00155   for (std::size_t i = 0; i < spec_.joint_bounds_.size(); ++i)
00156   {
00157     const robot_model::JointModel::Bounds& bounds = *spec_.joint_bounds_[i];
00158     for (std::size_t j = 0; j < bounds.size(); ++j)
00159     {
00160       m *= bounds[j].max_position_ - bounds[j].min_position_;
00161     }
00162   }
00163   return m;
00164 }
00165 
00166 double ompl_interface::ModelBasedStateSpace::distance(const ompl::base::State* state1,
00167                                                       const ompl::base::State* state2) const
00168 {
00169   if (distance_function_)
00170     return distance_function_(state1, state2);
00171   else
00172     return spec_.joint_model_group_->distance(state1->as<StateType>()->values, state2->as<StateType>()->values);
00173 }
00174 
00175 bool ompl_interface::ModelBasedStateSpace::equalStates(const ompl::base::State* state1,
00176                                                        const ompl::base::State* state2) const
00177 {
00178   for (unsigned int i = 0; i < variable_count_; ++i)
00179     if (fabs(state1->as<StateType>()->values[i] - state2->as<StateType>()->values[i]) >
00180         std::numeric_limits<double>::epsilon())
00181       return false;
00182   return true;
00183 }
00184 
00185 void ompl_interface::ModelBasedStateSpace::enforceBounds(ompl::base::State* state) const
00186 {
00187   spec_.joint_model_group_->enforcePositionBounds(state->as<StateType>()->values, spec_.joint_bounds_);
00188 }
00189 
00190 bool ompl_interface::ModelBasedStateSpace::satisfiesBounds(const ompl::base::State* state) const
00191 {
00192   return spec_.joint_model_group_->satisfiesPositionBounds(state->as<StateType>()->values, spec_.joint_bounds_,
00193                                                            std::numeric_limits<double>::epsilon());
00194 }
00195 
00196 void ompl_interface::ModelBasedStateSpace::interpolate(const ompl::base::State* from, const ompl::base::State* to,
00197                                                        const double t, ompl::base::State* state) const
00198 {
00199   // clear any cached info (such as validity known or not)
00200   state->as<StateType>()->clearKnownInformation();
00201 
00202   if (!interpolation_function_ || !interpolation_function_(from, to, t, state))
00203   {
00204     // perform the actual interpolation
00205     spec_.joint_model_group_->interpolate(from->as<StateType>()->values, to->as<StateType>()->values, t,
00206                                           state->as<StateType>()->values);
00207 
00208     // compute tag
00209     if (from->as<StateType>()->tag >= 0 && t < 1.0 - tag_snap_to_segment_)
00210       state->as<StateType>()->tag = from->as<StateType>()->tag;
00211     else if (to->as<StateType>()->tag >= 0 && t > tag_snap_to_segment_)
00212       state->as<StateType>()->tag = to->as<StateType>()->tag;
00213     else
00214       state->as<StateType>()->tag = -1;
00215   }
00216 }
00217 
00218 double* ompl_interface::ModelBasedStateSpace::getValueAddressAtIndex(ompl::base::State* state,
00219                                                                      const unsigned int index) const
00220 {
00221   if (index >= variable_count_)
00222     return NULL;
00223   return state->as<StateType>()->values + index;
00224 }
00225 
00226 void ompl_interface::ModelBasedStateSpace::setPlanningVolume(double minX, double maxX, double minY, double maxY,
00227                                                              double minZ, double maxZ)
00228 {
00229   for (std::size_t i = 0; i < joint_model_vector_.size(); ++i)
00230     if (joint_model_vector_[i]->getType() == robot_model::JointModel::PLANAR)
00231     {
00232       joint_bounds_storage_[i][0].min_position_ = minX;
00233       joint_bounds_storage_[i][0].max_position_ = maxX;
00234       joint_bounds_storage_[i][1].min_position_ = minY;
00235       joint_bounds_storage_[i][1].max_position_ = maxY;
00236     }
00237     else if (joint_model_vector_[i]->getType() == robot_model::JointModel::FLOATING)
00238     {
00239       joint_bounds_storage_[i][0].min_position_ = minX;
00240       joint_bounds_storage_[i][0].max_position_ = maxX;
00241       joint_bounds_storage_[i][1].min_position_ = minY;
00242       joint_bounds_storage_[i][1].max_position_ = maxY;
00243       joint_bounds_storage_[i][2].min_position_ = minZ;
00244       joint_bounds_storage_[i][2].max_position_ = maxZ;
00245     }
00246 }
00247 
00248 ompl::base::StateSamplerPtr ompl_interface::ModelBasedStateSpace::allocDefaultStateSampler() const
00249 {
00250   class DefaultStateSampler : public ompl::base::StateSampler
00251   {
00252   public:
00253     DefaultStateSampler(const ompl::base::StateSpace* space, const robot_model::JointModelGroup* group,
00254                         const robot_model::JointBoundsVector* joint_bounds)
00255       : ompl::base::StateSampler(space), joint_model_group_(group), joint_bounds_(joint_bounds)
00256     {
00257     }
00258 
00259     virtual void sampleUniform(ompl::base::State* state)
00260     {
00261       joint_model_group_->getVariableRandomPositions(moveit_rng_, state->as<StateType>()->values, *joint_bounds_);
00262       state->as<StateType>()->clearKnownInformation();
00263     }
00264 
00265     virtual void sampleUniformNear(ompl::base::State* state, const ompl::base::State* near, const double distance)
00266     {
00267       joint_model_group_->getVariableRandomPositionsNearBy(moveit_rng_, state->as<StateType>()->values, *joint_bounds_,
00268                                                            near->as<StateType>()->values, distance);
00269       state->as<StateType>()->clearKnownInformation();
00270     }
00271 
00272     virtual void sampleGaussian(ompl::base::State* state, const ompl::base::State* mean, const double stdDev)
00273     {
00274       sampleUniformNear(state, mean, rng_.gaussian(0.0, stdDev));
00275     }
00276 
00277   protected:
00278     random_numbers::RandomNumberGenerator moveit_rng_;
00279     const robot_model::JointModelGroup* joint_model_group_;
00280     const robot_model::JointBoundsVector* joint_bounds_;
00281   };
00282 
00283   return ompl::base::StateSamplerPtr(static_cast<ompl::base::StateSampler*>(
00284       new DefaultStateSampler(this, spec_.joint_model_group_, &spec_.joint_bounds_)));
00285 }
00286 
00287 void ompl_interface::ModelBasedStateSpace::printSettings(std::ostream& out) const
00288 {
00289   out << "ModelBasedStateSpace '" << getName() << "' at " << this << std::endl;
00290 }
00291 
00292 void ompl_interface::ModelBasedStateSpace::printState(const ompl::base::State* state, std::ostream& out) const
00293 {
00294   for (std::size_t j = 0; j < joint_model_vector_.size(); ++j)
00295   {
00296     out << joint_model_vector_[j]->getName() << " = ";
00297     const int idx = spec_.joint_model_group_->getVariableGroupIndex(joint_model_vector_[j]->getName());
00298     const int vc = joint_model_vector_[j]->getVariableCount();
00299     for (int i = 0; i < vc; ++i)
00300       out << state->as<StateType>()->values[idx + i] << " ";
00301     out << std::endl;
00302   }
00303 
00304   if (state->as<StateType>()->isStartState())
00305     out << "* start state" << std::endl;
00306   if (state->as<StateType>()->isGoalState())
00307     out << "* goal state" << std::endl;
00308   if (state->as<StateType>()->isValidityKnown())
00309   {
00310     if (state->as<StateType>()->isMarkedValid())
00311       out << "* valid state" << std::endl;
00312     else
00313       out << "* invalid state" << std::endl;
00314   }
00315   out << "Tag: " << state->as<StateType>()->tag << std::endl;
00316 }
00317 
00318 void ompl_interface::ModelBasedStateSpace::copyToRobotState(robot_state::RobotState& rstate,
00319                                                             const ompl::base::State* state) const
00320 {
00321   rstate.setJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
00322   rstate.update();
00323 }
00324 
00325 void ompl_interface::ModelBasedStateSpace::copyToOMPLState(ompl::base::State* state,
00326                                                            const robot_state::RobotState& rstate) const
00327 {
00328   rstate.copyJointGroupPositions(spec_.joint_model_group_, state->as<StateType>()->values);
00329   // clear any cached info (such as validity known or not)
00330   state->as<StateType>()->clearKnownInformation();
00331 }
00332 
00333 void ompl_interface::ModelBasedStateSpace::copyJointToOMPLState(ompl::base::State* state,
00334                                                                 const robot_state::RobotState& robot_state,
00335                                                                 const moveit::core::JointModel* joint_model,
00336                                                                 int ompl_state_joint_index) const
00337 {
00338   // Copy one joint (multiple variables possibly)
00339   memcpy(getValueAddressAtIndex(state, ompl_state_joint_index),
00340          robot_state.getVariablePositions() + joint_model->getFirstVariableIndex() * sizeof(double),
00341          joint_model->getVariableCount() * sizeof(double));
00342 
00343   // clear any cached info (such as validity known or not)
00344   state->as<StateType>()->clearKnownInformation();
00345 }


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jun 19 2019 19:24:27