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