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
00036
00037 #ifndef OMPL_CONTROL_SPACE_INFORMATION_
00038 #define OMPL_CONTROL_SPACE_INFORMATION_
00039
00040 #include "ompl/base/SpaceInformation.h"
00041 #include "ompl/control/ControlSpace.h"
00042 #include "ompl/control/ControlSampler.h"
00043 #include "ompl/control/DirectedControlSampler.h"
00044 #include "ompl/control/StatePropagator.h"
00045 #include "ompl/control/Control.h"
00046 #include "ompl/util/ClassForward.h"
00047
00048 namespace ompl
00049 {
00050
00053 namespace control
00054 {
00055
00057
00058 ClassForward(SpaceInformation);
00060
00066 typedef boost::function<void(const base::State*, const Control*, const double, base::State*)> StatePropagatorFn;
00067
00069 class SpaceInformation : public base::SpaceInformation
00070 {
00071 public:
00072
00074 SpaceInformation(const base::StateSpacePtr &stateSpace, const ControlSpacePtr &controlSpace) :
00075 base::SpaceInformation(stateSpace), controlSpace_(controlSpace),
00076 minSteps_(0), maxSteps_(0), stepSize_(0.0)
00077 {
00078 }
00079
00080 virtual ~SpaceInformation(void)
00081 {
00082 }
00083
00085 const ControlSpacePtr& getControlSpace(void) const
00086 {
00087 return controlSpace_;
00088 }
00089
00094 Control* allocControl(void) const
00095 {
00096 return controlSpace_->allocControl();
00097 }
00098
00100 void freeControl(Control *control) const
00101 {
00102 controlSpace_->freeControl(control);
00103 }
00104
00106 void copyControl(Control *destination, const Control *source) const
00107 {
00108 controlSpace_->copyControl(destination, source);
00109 }
00110
00112 Control* cloneControl(const Control *source) const
00113 {
00114 Control *copy = controlSpace_->allocControl();
00115 controlSpace_->copyControl(copy, source);
00116 return copy;
00117 }
00118
00125 void printControl(const Control *control, std::ostream &out = std::cout) const
00126 {
00127 controlSpace_->printControl(control, out);
00128 }
00129
00131 bool equalControls(const Control *control1, const Control *control2) const
00132 {
00133 return controlSpace_->equalControls(control1, control2);
00134 }
00135
00137 void nullControl(Control *control) const
00138 {
00139 controlSpace_->nullControl(control);
00140 }
00141
00148 ControlSamplerPtr allocControlSampler(void) const
00149 {
00150 return controlSpace_->allocControlSampler();
00151 }
00152
00154 void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
00155 {
00156 minSteps_ = minSteps;
00157 maxSteps_ = maxSteps;
00158 }
00159
00161 unsigned int getMinControlDuration(void) const
00162 {
00163 return minSteps_;
00164 }
00165
00167 unsigned int getMaxControlDuration(void) const
00168 {
00169 return maxSteps_;
00170 }
00171
00174 DirectedControlSamplerPtr allocDirectedControlSampler(void) const;
00175
00177 void setDirectedControlSamplerAllocator(const DirectedControlSamplerAllocator &dcsa);
00178
00180 void clearDirectedSamplerAllocator(void);
00181
00188 const StatePropagatorPtr& getStatePropagator(void) const
00189 {
00190 return statePropagator_;
00191 }
00192
00194 void setStatePropagator(const StatePropagatorFn &fn);
00195
00197 void setStatePropagator(const StatePropagatorPtr &sp);
00198
00201 void setPropagationStepSize(double stepSize)
00202 {
00203 stepSize_ = stepSize;
00204 }
00205
00207 double getPropagationStepSize(void) const
00208 {
00209 return stepSize_;
00210 }
00221 void propagate(const base::State *state, const Control* control, int steps, base::State *result) const;
00222
00227 bool canPropagateBackward(void) const;
00228
00236 unsigned int propagateWhileValid(const base::State *state, const Control* control, int steps, base::State *result) const;
00237
00246 void propagate(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const;
00247
00260 unsigned int propagateWhileValid(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const;
00261
00265 virtual void printSettings(std::ostream &out = std::cout) const;
00266
00268 virtual void setup(void);
00269
00270 protected:
00271
00273 ControlSpacePtr controlSpace_;
00274
00276 StatePropagatorPtr statePropagator_;
00277
00279 unsigned int minSteps_;
00280
00282 unsigned int maxSteps_;
00283
00285 DirectedControlSamplerAllocator dcsa_;
00286
00288 double stepSize_;
00289
00290 };
00291
00292 }
00293
00294 }
00295
00296 #endif