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/spaces/RealVectorControlSpace.h"
00038 #include "ompl/util/Exception.h"
00039 #include <boost/lexical_cast.hpp>
00040 #include <cstring>
00041 #include <limits>
00042
00043 void ompl::control::RealVectorControlUniformSampler::sample(Control *control)
00044 {
00045 const unsigned int dim = space_->getDimension();
00046 const base::RealVectorBounds &bounds = static_cast<const RealVectorControlSpace*>(space_)->getBounds();
00047
00048 RealVectorControlSpace::ControlType *rcontrol = static_cast<RealVectorControlSpace::ControlType*>(control);
00049 for (unsigned int i = 0 ; i < dim ; ++i)
00050 rcontrol->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
00051 }
00052
00053 void ompl::control::RealVectorControlSpace::setup(void)
00054 {
00055 ControlSpace::setup();
00056 bounds_.check();
00057 }
00058
00059 void ompl::control::RealVectorControlSpace::setBounds(const base::RealVectorBounds &bounds)
00060 {
00061 bounds.check();
00062 if (bounds.low.size() != dimension_)
00063 throw Exception("Bounds do not match dimension of control space: expected dimension " +
00064 boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
00065 boost::lexical_cast<std::string>(bounds.low.size()));
00066 bounds_ = bounds;
00067 }
00068
00069 unsigned int ompl::control::RealVectorControlSpace::getDimension(void) const
00070 {
00071 return dimension_;
00072 }
00073
00074 void ompl::control::RealVectorControlSpace::copyControl(Control *destination, const Control *source) const
00075 {
00076 memcpy(static_cast<ControlType*>(destination)->values,
00077 static_cast<const ControlType*>(source)->values, controlBytes_);
00078 }
00079
00080 bool ompl::control::RealVectorControlSpace::equalControls(const Control *control1, const Control *control2) const
00081 {
00082 const double *s1 = static_cast<const ControlType*>(control1)->values;
00083 const double *s2 = static_cast<const ControlType*>(control2)->values;
00084 for (unsigned int i = 0 ; i < dimension_ ; ++i)
00085 {
00086 double diff = (*s1++) - (*s2++);
00087 if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
00088 return false;
00089 }
00090 return true;
00091 }
00092
00093 ompl::control::ControlSamplerPtr ompl::control::RealVectorControlSpace::allocDefaultControlSampler(void) const
00094 {
00095 return ControlSamplerPtr(new RealVectorControlUniformSampler(this));
00096 }
00097
00098 ompl::control::Control* ompl::control::RealVectorControlSpace::allocControl(void) const
00099 {
00100 ControlType *rcontrol = new ControlType();
00101 rcontrol->values = new double[dimension_];
00102 return rcontrol;
00103 }
00104
00105 void ompl::control::RealVectorControlSpace::freeControl(Control *control) const
00106 {
00107 ControlType *rcontrol = static_cast<ControlType*>(control);
00108 delete[] rcontrol->values;
00109 delete rcontrol;
00110 }
00111
00112 void ompl::control::RealVectorControlSpace::nullControl(Control *control) const
00113 {
00114 ControlType *rcontrol = static_cast<ControlType*>(control);
00115 for (unsigned int i = 0 ; i < dimension_ ; ++i)
00116 {
00117 if (bounds_.low[i] <= 0.0 && bounds_.high[i] >= 0.0)
00118 rcontrol->values[i] = 0.0;
00119 else
00120 rcontrol->values[i] = bounds_.low[i];
00121 }
00122 }
00123
00124 double* ompl::control::RealVectorControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
00125 {
00126 return index < dimension_ ? static_cast<ControlType*>(control)->values + index : NULL;
00127 }
00128
00129 void ompl::control::RealVectorControlSpace::printControl(const Control *control, std::ostream &out) const
00130 {
00131 out << "RealVectorControl [";
00132 if (control)
00133 {
00134 const ControlType *rcontrol = static_cast<const ControlType*>(control);
00135 for (unsigned int i = 0 ; i < dimension_ ; ++i)
00136 {
00137 out << rcontrol->values[i];
00138 if (i + 1 < dimension_)
00139 out << ' ';
00140 }
00141 }
00142 else
00143 out << "NULL";
00144 out << ']' << std::endl;
00145 }
00146
00147 void ompl::control::RealVectorControlSpace::printSettings(std::ostream &out) const
00148 {
00149 out << "Real vector control space '" << getName() << "' with bounds: " << std::endl;
00150 out << " - min: ";
00151 for (unsigned int i = 0 ; i < dimension_ ; ++i)
00152 out << bounds_.low[i] << " ";
00153 out << std::endl;
00154 out << " - max: ";
00155 for (unsigned int i = 0 ; i < dimension_ ; ++i)
00156 out << bounds_.high[i] << " ";
00157 out << std::endl;
00158 }
00159
00160 unsigned int ompl::control::RealVectorControlSpace::getSerializationLength(void) const
00161 {
00162 return controlBytes_;
00163 }
00164
00165 void ompl::control::RealVectorControlSpace::serialize(void *serialization, const Control *ctrl) const
00166 {
00167 memcpy(serialization, ctrl->as<ControlType>()->values, controlBytes_);
00168 }
00169
00170 void ompl::control::RealVectorControlSpace::deserialize(Control *ctrl, const void *serialization) const
00171 {
00172 memcpy(ctrl->as<ControlType>()->values, serialization, controlBytes_);
00173 }