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/StatePropagator.h"
00044 #include "ompl/control/Control.h"
00045 #include "ompl/util/ClassForward.h"
00046
00047 namespace ompl
00048 {
00049
00052 namespace control
00053 {
00054
00056 ClassForward(SpaceInformation);
00057
00063 typedef boost::function4<void, const base::State*, const Control*, const double, base::State*> StatePropagatorFn;
00064
00066 class SpaceInformation : public base::SpaceInformation
00067 {
00068 public:
00069
00071 SpaceInformation(const base::StateSpacePtr &stateSpace, const ControlSpacePtr &controlSpace) :
00072 base::SpaceInformation(stateSpace), controlSpace_(controlSpace),
00073 minSteps_(0), maxSteps_(0), stepSize_(0.0)
00074 {
00075 }
00076
00077 virtual ~SpaceInformation(void)
00078 {
00079 }
00080
00082 const ControlSpacePtr& getControlSpace(void) const
00083 {
00084 return controlSpace_;
00085 }
00086
00091 Control* allocControl(void) const
00092 {
00093 return controlSpace_->allocControl();
00094 }
00095
00097 void freeControl(Control *control) const
00098 {
00099 controlSpace_->freeControl(control);
00100 }
00101
00103 void copyControl(Control *destination, const Control *source) const
00104 {
00105 controlSpace_->copyControl(destination, source);
00106 }
00107
00109 Control* cloneControl(const Control *source) const
00110 {
00111 Control *copy = controlSpace_->allocControl();
00112 controlSpace_->copyControl(copy, source);
00113 return copy;
00114 }
00115
00122 void printControl(const Control *control, std::ostream &out = std::cout) const
00123 {
00124 controlSpace_->printControl(control, out);
00125 }
00126
00128 bool equalControls(const Control *control1, const Control *control2) const
00129 {
00130 return controlSpace_->equalControls(control1, control2);
00131 }
00132
00134 void nullControl(Control *control) const
00135 {
00136 controlSpace_->nullControl(control);
00137 }
00138
00145 ControlSamplerPtr allocControlSampler(void) const
00146 {
00147 return controlSpace_->allocControlSampler();
00148 }
00149
00151 void setMinMaxControlDuration(unsigned int minSteps, unsigned int maxSteps)
00152 {
00153 minSteps_ = minSteps;
00154 maxSteps_ = maxSteps;
00155 }
00156
00158 unsigned int getMinControlDuration(void) const
00159 {
00160 return minSteps_;
00161 }
00162
00164 unsigned int getMaxControlDuration(void) const
00165 {
00166 return maxSteps_;
00167 }
00174 const StatePropagatorPtr& getStatePropagator(void) const
00175 {
00176 return statePropagator_;
00177 }
00178
00180 void setStatePropagator(const StatePropagatorFn &fn);
00181
00183 void setStatePropagator(const StatePropagatorPtr &sp);
00184
00187 void setPropagationStepSize(double stepSize)
00188 {
00189 stepSize_ = stepSize;
00190 }
00191
00193 double getPropagationStepSize(void) const
00194 {
00195 return stepSize_;
00196 }
00207 void propagate(const base::State *state, const Control* control, int steps, base::State *result) const;
00208
00213 bool canPropagateBackward(void) const;
00214
00222 unsigned int propagateWhileValid(const base::State *state, const Control* control, int steps, base::State *result) const;
00223
00232 void propagate(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const;
00233
00246 unsigned int propagateWhileValid(const base::State *state, const Control* control, int steps, std::vector<base::State*> &result, bool alloc) const;
00247
00251 virtual void printSettings(std::ostream &out = std::cout) const;
00252
00254 virtual void setup(void);
00255
00256 protected:
00257
00259 ControlSpacePtr controlSpace_;
00260
00262 StatePropagatorPtr statePropagator_;
00263
00265 unsigned int minSteps_;
00266
00268 unsigned int maxSteps_;
00269
00271 double stepSize_;
00272
00273 };
00274
00275 }
00276
00277 }
00278
00279 #endif