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