model_based_state_space.h
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 #ifndef MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_
00038 #define MOVEIT_OMPL_INTERFACE_PARAMETERIZATION_MODEL_BASED_STATE_SPACE_
00039 
00040 #include <ompl/base/StateSpace.h>
00041 #include <moveit/robot_model/robot_model.h>
00042 #include <moveit/robot_state/robot_state.h>
00043 #include <moveit/kinematic_constraints/kinematic_constraint.h>
00044 #include <moveit/constraint_samplers/constraint_sampler.h>
00045 
00046 namespace ompl_interface
00047 {
00048 typedef boost::function<bool(const ompl::base::State* from, const ompl::base::State* to, const double t,
00049                              ompl::base::State* state)>
00050     InterpolationFunction;
00051 typedef boost::function<double(const ompl::base::State* state1, const ompl::base::State* state2)> DistanceFunction;
00052 
00053 struct ModelBasedStateSpaceSpecification
00054 {
00055   ModelBasedStateSpaceSpecification(const robot_model::RobotModelConstPtr& robot_model,
00056                                     const robot_model::JointModelGroup* jmg)
00057     : robot_model_(robot_model), joint_model_group_(jmg)
00058   {
00059   }
00060 
00061   ModelBasedStateSpaceSpecification(const robot_model::RobotModelConstPtr& robot_model, const std::string& group_name)
00062     : robot_model_(robot_model), joint_model_group_(robot_model_->getJointModelGroup(group_name))
00063   {
00064     if (!joint_model_group_)
00065       throw std::runtime_error("Group '" + group_name + "'  was not found");
00066   }
00067 
00068   robot_model::RobotModelConstPtr robot_model_;
00069   const robot_model::JointModelGroup* joint_model_group_;
00070   robot_model::JointBoundsVector joint_bounds_;
00071 };
00072 
00073 class ModelBasedStateSpace : public ompl::base::StateSpace
00074 {
00075 public:
00076   class StateType : public ompl::base::State
00077   {
00078   public:
00079     enum
00080     {
00081       VALIDITY_KNOWN = 1,
00082       GOAL_DISTANCE_KNOWN = 2,
00083       VALIDITY_TRUE = 4,
00084       IS_START_STATE = 8,
00085       IS_GOAL_STATE = 16
00086     };
00087 
00088     StateType() : ompl::base::State(), values(NULL), tag(-1), flags(0), distance(0.0)
00089     {
00090     }
00091 
00092     void markValid(double d)
00093     {
00094       distance = d;
00095       flags |= GOAL_DISTANCE_KNOWN;
00096       markValid();
00097     }
00098 
00099     void markValid()
00100     {
00101       flags |= (VALIDITY_KNOWN | VALIDITY_TRUE);
00102     }
00103 
00104     void markInvalid(double d)
00105     {
00106       distance = d;
00107       flags |= GOAL_DISTANCE_KNOWN;
00108       markInvalid();
00109     }
00110 
00111     void markInvalid()
00112     {
00113       flags &= ~VALIDITY_TRUE;
00114       flags |= VALIDITY_KNOWN;
00115     }
00116 
00117     bool isValidityKnown() const
00118     {
00119       return flags & VALIDITY_KNOWN;
00120     }
00121 
00122     void clearKnownInformation()
00123     {
00124       flags = 0;
00125     }
00126 
00127     bool isMarkedValid() const
00128     {
00129       return flags & VALIDITY_TRUE;
00130     }
00131 
00132     bool isGoalDistanceKnown() const
00133     {
00134       return flags & GOAL_DISTANCE_KNOWN;
00135     }
00136 
00137     bool isStartState() const
00138     {
00139       return flags & IS_START_STATE;
00140     }
00141 
00142     bool isGoalState() const
00143     {
00144       return flags & IS_GOAL_STATE;
00145     }
00146 
00147     bool isInputState() const
00148     {
00149       return flags & (IS_START_STATE | IS_GOAL_STATE);
00150     }
00151 
00152     void markStartState()
00153     {
00154       flags |= IS_START_STATE;
00155     }
00156 
00157     void markGoalState()
00158     {
00159       flags |= IS_GOAL_STATE;
00160     }
00161 
00162     double* values;
00163     int tag;
00164     int flags;
00165     double distance;
00166   };
00167 
00168   ModelBasedStateSpace(const ModelBasedStateSpaceSpecification& spec);
00169   virtual ~ModelBasedStateSpace();
00170 
00171   void setInterpolationFunction(const InterpolationFunction& fun)
00172   {
00173     interpolation_function_ = fun;
00174   }
00175 
00176   void setDistanceFunction(const DistanceFunction& fun)
00177   {
00178     distance_function_ = fun;
00179   }
00180 
00181   virtual ompl::base::State* allocState() const;
00182   virtual void freeState(ompl::base::State* state) const;
00183   virtual unsigned int getDimension() const;
00184   virtual void enforceBounds(ompl::base::State* state) const;
00185   virtual bool satisfiesBounds(const ompl::base::State* state) const;
00186 
00187   virtual void copyState(ompl::base::State* destination, const ompl::base::State* source) const;
00188   virtual void interpolate(const ompl::base::State* from, const ompl::base::State* to, const double t,
00189                            ompl::base::State* state) const;
00190   virtual double distance(const ompl::base::State* state1, const ompl::base::State* state2) const;
00191   virtual bool equalStates(const ompl::base::State* state1, const ompl::base::State* state2) const;
00192   virtual double getMaximumExtent() const;
00193   virtual double getMeasure() const;
00194 
00195   virtual unsigned int getSerializationLength() const;
00196   virtual void serialize(void* serialization, const ompl::base::State* state) const;
00197   virtual void deserialize(ompl::base::State* state, const void* serialization) const;
00198   virtual double* getValueAddressAtIndex(ompl::base::State* state, const unsigned int index) const;
00199 
00200   virtual ompl::base::StateSamplerPtr allocDefaultStateSampler() const;
00201 
00202   const robot_model::RobotModelConstPtr& getRobotModel() const
00203   {
00204     return spec_.robot_model_;
00205   }
00206 
00207   const robot_model::JointModelGroup* getJointModelGroup() const
00208   {
00209     return spec_.joint_model_group_;
00210   }
00211 
00212   const std::string& getJointModelGroupName() const
00213   {
00214     return getJointModelGroup()->getName();
00215   }
00216 
00217   const ModelBasedStateSpaceSpecification& getSpecification() const
00218   {
00219     return spec_;
00220   }
00221 
00222   virtual void printState(const ompl::base::State* state, std::ostream& out) const;
00223   virtual void printSettings(std::ostream& out) const;
00224 
00226   virtual void setPlanningVolume(double minX, double maxX, double minY, double maxY, double minZ, double maxZ);
00227 
00228   const robot_model::JointBoundsVector& getJointsBounds() const
00229   {
00230     return spec_.joint_bounds_;
00231   }
00232 
00234   // The joint states \b must be specified in the same order as the joint models in the constructor
00235   virtual void copyToRobotState(robot_state::RobotState& rstate, const ompl::base::State* state) const;
00236 
00238   //  The joint states \b must be specified in the same order as the joint models in the constructor
00239   virtual void copyToOMPLState(ompl::base::State* state, const robot_state::RobotState& rstate) const;
00240 
00251   virtual void copyJointToOMPLState(ompl::base::State* state, const robot_state::RobotState& robot_state,
00252                                     const moveit::core::JointModel* joint_model, int ompl_state_joint_index) const;
00253 
00254   double getTagSnapToSegment() const;
00255   void setTagSnapToSegment(double snap);
00256 
00257 protected:
00258   ModelBasedStateSpaceSpecification spec_;
00259   std::vector<robot_model::JointModel::Bounds> joint_bounds_storage_;
00260   std::vector<const robot_model::JointModel*> joint_model_vector_;
00261   unsigned int variable_count_;
00262   size_t state_values_size_;
00263 
00264   InterpolationFunction interpolation_function_;
00265   DistanceFunction distance_function_;
00266 
00267   double tag_snap_to_segment_;
00268   double tag_snap_to_segment_complement_;
00269 };
00270 
00271 typedef boost::shared_ptr<ModelBasedStateSpace> ModelBasedStateSpacePtr;
00272 }
00273 
00274 #endif


ompl
Author(s): Ioan Sucan
autogenerated on Wed Jan 17 2018 03:32:20