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/Control.h"
00044 #include "ompl/util/ClassForward.h"
00045
00046 namespace ompl
00047 {
00048
00051 namespace control
00052 {
00053
00055 ClassForward(SpaceInformation);
00056
00061 class SpaceInformation : public base::SpaceInformation
00062 {
00063 public:
00064
00066 SpaceInformation(const base::StateSpacePtr &stateSpace, const ControlSpacePtr &controlSpace) :
00067 base::SpaceInformation(stateSpace), controlSpace_(controlSpace),
00068 minSteps_(0), maxSteps_(0), stepSize_(0.0)
00069 {
00070 }
00071
00072 virtual ~SpaceInformation(void)
00073 {
00074 }
00075
00077 const ControlSpacePtr& getControlSpace(void) const
00078 {
00079 return controlSpace_;
00080 }
00081
00086 Control* allocControl(void) const
00087 {
00088 return controlSpace_->allocControl();
00089 }
00090
00092 void freeControl(Control *control) const
00093 {
00094 controlSpace_->freeControl(control);
00095 }
00096
00098 void copyControl(Control *destination, const Control *source) const
00099 {
00100 controlSpace_->copyControl(destination, source);
00101 }
00102
00104 Control* cloneControl(const Control *source) const
00105 {
00106 Control *copy = controlSpace_->allocControl();
00107 controlSpace_->copyControl(copy, source);
00108 return copy;
00109 }
00110
00117 void printControl(const Control *control, std::ostream &out = std::cout) const
00118 {
00119 controlSpace_->printControl(control, out);
00120 }
00121
00123 bool equalControls(const Control *control1, const Control *control2) const
00124 {
00125 return controlSpace_->equalControls(control1, control2);
00126 }
00127
00129 void nullControl(Control *control) const
00130 {
00131 controlSpace_->nullControl(control);
00132 }
00133
00140 ControlSamplerPtr allocControlSampler(void) const
00141 {
00142 return controlSpace_->allocControlSampler();
00143 }
00144
00147 void setPropagationStepSize(double stepSize)
00148 {
00149 stepSize_ = stepSize;
00150 }
00151
00153 double getPropagationStepSize(void) const
00154 {
00155 return stepSize_;
00156 }
00157
00159 void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
00160 {
00161 minSteps_ = minSteps;
00162 maxSteps_ = maxSteps;
00163 }
00164
00166 unsigned int getMinControlDuration(void) const
00167 {
00168 return minSteps_;
00169 }
00170
00172 unsigned int getMaxControlDuration(void) const
00173 {
00174 return maxSteps_;
00175 }
00186 void propagate(const base::State *state, const Control* control, unsigned int steps, base::State *result) const;
00187
00195 unsigned int propagateWhileValid(const base::State *state, const Control* control, unsigned int steps, base::State *result) const;
00196
00205 void propagate(const base::State *state, const Control* control, unsigned int steps, std::vector<base::State*> &result, bool alloc) const;
00206
00219 unsigned int propagateWhileValid(const base::State *state, const Control* control, unsigned int steps, std::vector<base::State*> &result, bool alloc) const;
00220
00224 virtual void printSettings(std::ostream &out = std::cout) const;
00225
00227 virtual void setup(void);
00228
00229 protected:
00230
00232 ControlSpacePtr controlSpace_;
00233
00235 unsigned int minSteps_;
00236
00238 unsigned int maxSteps_;
00239
00241 double stepSize_;
00242
00243 };
00244
00245 }
00246
00247 }
00248
00249 #endif