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/DiscreteControlSpace.h"
00038 #include "ompl/util/Exception.h"
00039
00040 void ompl::control::DiscreteControlSampler::sample(Control *control)
00041 {
00042 control->as<DiscreteControlSpace::ControlType>()->value =
00043 rng_.uniformInt(space_->as<DiscreteControlSpace>()->getLowerBound(),
00044 space_->as<DiscreteControlSpace>()->getUpperBound());
00045 }
00046
00047 unsigned int ompl::control::DiscreteControlSpace::getDimension(void) const
00048 {
00049 return 1;
00050 }
00051
00052 void ompl::control::DiscreteControlSpace::copyControl(Control *destination, const Control *source) const
00053 {
00054 destination->as<ControlType>()->value = source->as<ControlType>()->value;
00055 }
00056
00057 bool ompl::control::DiscreteControlSpace::equalControls(const Control *control1, const Control *control2) const
00058 {
00059 return control1->as<ControlType>()->value == control2->as<ControlType>()->value;
00060 }
00061
00062 ompl::control::ControlSamplerPtr ompl::control::DiscreteControlSpace::allocDefaultControlSampler(void) const
00063 {
00064 return ControlSamplerPtr(new DiscreteControlSampler(this));
00065 }
00066
00067 ompl::control::Control* ompl::control::DiscreteControlSpace::allocControl(void) const
00068 {
00069 return new ControlType();
00070 }
00071
00072 void ompl::control::DiscreteControlSpace::freeControl(Control *control) const
00073 {
00074 delete static_cast<ControlType*>(control);
00075 }
00076
00077 void ompl::control::DiscreteControlSpace::nullControl(Control *control) const
00078 {
00079 control->as<ControlType>()->value = lowerBound_;
00080 }
00081
00082 void ompl::control::DiscreteControlSpace::printControl(const Control *control, std::ostream &out) const
00083 {
00084 out << "DiscreteControl [";
00085 if (control)
00086 out << control->as<ControlType>()->value;
00087 else
00088 out << "NULL";
00089 out << ']' << std::endl;
00090 }
00091
00092 void ompl::control::DiscreteControlSpace::printSettings(std::ostream &out) const
00093 {
00094 out << "Discrete control space '" << getName() << "' with bounds [" << lowerBound_ << ", " << upperBound_ << "]" << std::endl;
00095 }
00096
00097 void ompl::control::DiscreteControlSpace::setup(void)
00098 {
00099 if (lowerBound_ > upperBound_)
00100 throw Exception("Lower bound cannot be larger than upper bound for a discrete space");
00101 ControlSpace::setup();
00102 }
00103
00104 unsigned int ompl::control::DiscreteControlSpace::getSerializationLength(void) const
00105 {
00106 return sizeof(int);
00107 }
00108
00109 void ompl::control::DiscreteControlSpace::serialize(void *serialization, const Control *ctrl) const
00110 {
00111 memcpy(serialization, &ctrl->as<ControlType>()->value, sizeof(int));
00112 }
00113
00114 void ompl::control::DiscreteControlSpace::deserialize(Control *ctrl, const void *serialization) const
00115 {
00116 memcpy(&ctrl->as<ControlType>()->value, serialization, sizeof(int));
00117 }