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 #include "ompl/control/ControlManifold.h"
00038 #include "ompl/util/Exception.h"
00039
00040 ompl::control::ControlManifold::ControlManifold(const base::StateManifoldPtr &stateManifold) : stateManifold_(stateManifold)
00041 {
00042 name_ = "Control[" + stateManifold_->getName() + "]";
00043 }
00044
00045 ompl::control::ControlManifold::~ControlManifold(void)
00046 {
00047 }
00048
00049 const std::string& ompl::control::ControlManifold::getName(void) const
00050 {
00051 return name_;
00052 }
00053
00054 void ompl::control::ControlManifold::setName(const std::string &name)
00055 {
00056 name_ = name;
00057 }
00058
00059 void ompl::control::ControlManifold::setup(void)
00060 {
00061 }
00062
00063 bool ompl::control::ControlManifold::canPropagateBackward(void) const
00064 {
00065 return true;
00066 }
00067
00068 double* ompl::control::ControlManifold::getValueAddressAtIndex(Control *control, const unsigned int index) const
00069 {
00070 return NULL;
00071 }
00072
00073 void ompl::control::ControlManifold::printControl(const Control *control, std::ostream &out) const
00074 {
00075 out << "Control instance: " << control << std::endl;
00076 }
00077
00078 void ompl::control::ControlManifold::printSettings(std::ostream &out) const
00079 {
00080 out << "ControlManifold '" << getName() << "' instance: " << this << std::endl;
00081 }
00082
00083 void ompl::control::ControlManifold::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00084 {
00085 if (statePropagation_)
00086 statePropagation_(state, control, duration, result);
00087 else
00088 throw Exception("State propagation routine is not set for control manifold. Either set this routine or provide a different implementation in an inherited class.");
00089 }
00090
00091 void ompl::control::ControlManifold::setPropagationFunction(const StatePropagationFn &fn)
00092 {
00093 statePropagation_ = fn;
00094 }
00095
00096 void ompl::control::CompoundControlManifold::addSubManifold(const ControlManifoldPtr &component)
00097 {
00098 if (locked_)
00099 throw Exception("This manifold is locked. No further components can be added");
00100
00101 components_.push_back(component);
00102 componentCount_ = components_.size();
00103 }
00104
00105 unsigned int ompl::control::CompoundControlManifold::getSubManifoldCount(void) const
00106 {
00107 return componentCount_;
00108 }
00109
00110 const ompl::control::ControlManifoldPtr& ompl::control::CompoundControlManifold::getSubManifold(const unsigned int index) const
00111 {
00112 if (componentCount_ > index)
00113 return components_[index];
00114 else
00115 throw Exception("Submanifold index does not exist");
00116 }
00117
00118 const ompl::control::ControlManifoldPtr& ompl::control::CompoundControlManifold::getSubManifold(const std::string &name) const
00119 {
00120 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00121 if (components_[i]->getName() == name)
00122 return components_[i];
00123 throw Exception("Submanifold " + name + " does not exist");
00124 }
00125
00126 unsigned int ompl::control::CompoundControlManifold::getDimension(void) const
00127 {
00128 unsigned int dim = 0;
00129 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00130 dim += components_[i]->getDimension();
00131 return dim;
00132 }
00133
00134 ompl::control::Control* ompl::control::CompoundControlManifold::allocControl(void) const
00135 {
00136 CompoundControl *control = new CompoundControl();
00137 control->components = new Control*[componentCount_];
00138 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00139 control->components[i] = components_[i]->allocControl();
00140 return static_cast<Control*>(control);
00141 }
00142
00143 void ompl::control::CompoundControlManifold::freeControl(Control *control) const
00144 {
00145 CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00146 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00147 components_[i]->freeControl(ccontrol->components[i]);
00148 delete[] ccontrol->components;
00149 delete ccontrol;
00150 }
00151
00152 void ompl::control::CompoundControlManifold::copyControl(Control *destination, const Control *source) const
00153 {
00154 CompoundControl *cdest = static_cast<CompoundControl*>(destination);
00155 const CompoundControl *csrc = static_cast<const CompoundControl*>(source);
00156 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00157 components_[i]->copyControl(cdest->components[i], csrc->components[i]);
00158 }
00159
00160 bool ompl::control::CompoundControlManifold::equalControls(const Control *control1, const Control *control2) const
00161 {
00162 const CompoundControl *ccontrol1 = static_cast<const CompoundControl*>(control1);
00163 const CompoundControl *ccontrol2 = static_cast<const CompoundControl*>(control2);
00164 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00165 if (!components_[i]->equalControls(ccontrol1->components[i], ccontrol2->components[i]))
00166 return false;
00167 return true;
00168 }
00169
00170 void ompl::control::CompoundControlManifold::nullControl(Control *control) const
00171 {
00172 CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00173 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00174 components_[i]->nullControl(ccontrol->components[i]);
00175 }
00176
00177 ompl::control::ControlSamplerPtr ompl::control::CompoundControlManifold::allocControlSampler(void) const
00178 {
00179 CompoundControlSampler *ss = new CompoundControlSampler(this);
00180 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00181 ss->addSampler(components_[i]->allocControlSampler());
00182 return ControlSamplerPtr(ss);
00183 }
00184
00185 void ompl::control::CompoundControlManifold::propagate(const base::State *state, const Control* control, const double duration, base::State *result) const
00186 {
00187 if (statePropagation_)
00188 statePropagation_(state, control, duration, result);
00189 else
00190 {
00191 const base::CompoundState *cstate = static_cast<const base::CompoundState*>(state);
00192 const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
00193 base::CompoundState *cresult = static_cast<base::CompoundState*>(result);
00194 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00195 components_[i]->propagate(cstate->components[i], ccontrol->components[i], duration, cresult->components[i]);
00196 }
00197 }
00198
00199 void ompl::control::CompoundControlManifold::lock(void)
00200 {
00201 locked_ = true;
00202 }
00203
00204 bool ompl::control::CompoundControlManifold::canPropagateBackward(void) const
00205 {
00206 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00207 if (!components_[i]->canPropagateBackward())
00208 return false;
00209 return true;
00210 }
00211
00212 double* ompl::control::CompoundControlManifold::getValueAddressAtIndex(Control *control, const unsigned int index) const
00213 {
00214 CompoundControl *ccontrol = static_cast<CompoundControl*>(control);
00215 unsigned int idx = 0;
00216
00217 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00218 for (unsigned int j = 0 ; j <= index ; ++j)
00219 {
00220 double *va = components_[i]->getValueAddressAtIndex(ccontrol->components[i], j);
00221 if (va)
00222 {
00223 if (idx == index)
00224 return va;
00225 else
00226 idx++;
00227 }
00228 else
00229 break;
00230 }
00231 return NULL;
00232 }
00233
00234 void ompl::control::CompoundControlManifold::printControl(const Control *control, std::ostream &out) const
00235 {
00236 out << "Compound control [" << std::endl;
00237 const CompoundControl *ccontrol = static_cast<const CompoundControl*>(control);
00238 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00239 components_[i]->printControl(ccontrol->components[i], out);
00240 out << "]" << std::endl;
00241 }
00242
00243 void ompl::control::CompoundControlManifold::printSettings(std::ostream &out) const
00244 {
00245 out << "Compound control manifold '" << getName() << "' [" << std::endl;
00246 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00247 components_[i]->printSettings(out);
00248 out << "]" << std::endl;
00249 }
00250
00251 void ompl::control::CompoundControlManifold::setup(void)
00252 {
00253 for (unsigned int i = 0 ; i < componentCount_ ; ++i)
00254 components_[i]->setup();
00255 ControlManifold::setup();
00256 }