RealVectorControlSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2010, Rice University
5 * All rights reserved.
6 *
7 * Redistribution and use in source and binary forms, with or without
8 * modification, are permitted provided that the following conditions
9 * are met:
10 *
11 * * Redistributions of source code must retain the above copyright
12 * notice, this list of conditions and the following disclaimer.
13 * * Redistributions in binary form must reproduce the above
14 * copyright notice, this list of conditions and the following
15 * disclaimer in the documentation and/or other materials provided
16 * with the distribution.
17 * * Neither the name of the Rice University nor the names of its
18 * contributors may be used to endorse or promote products derived
19 * from this software without specific prior written permission.
20 *
21 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 * POSSIBILITY OF SUCH DAMAGE.
33 *********************************************************************/
34 
35 /* Author: Ioan Sucan */
36 
37 #include "ompl/control/spaces/RealVectorControlSpace.h"
38 #include "ompl/util/Exception.h"
39 #include <boost/lexical_cast.hpp>
40 #include <cstring>
41 #include <limits>
42 
44 {
45  const unsigned int dim = space_->getDimension();
46  const base::RealVectorBounds &bounds = static_cast<const RealVectorControlSpace*>(space_)->getBounds();
47 
49  for (unsigned int i = 0 ; i < dim ; ++i)
50  rcontrol->values[i] = rng_.uniformReal(bounds.low[i], bounds.high[i]);
51 }
52 
54 {
56  bounds_.check();
57 }
58 
60 {
61  bounds.check();
62  if (bounds.low.size() != dimension_)
63  throw Exception("Bounds do not match dimension of control space: expected dimension " +
64  boost::lexical_cast<std::string>(dimension_) + " but got dimension " +
65  boost::lexical_cast<std::string>(bounds.low.size()));
66  bounds_ = bounds;
67 }
68 
70 {
71  return dimension_;
72 }
73 
75 {
76  memcpy(static_cast<ControlType*>(destination)->values,
77  static_cast<const ControlType*>(source)->values, controlBytes_);
78 }
79 
80 bool ompl::control::RealVectorControlSpace::equalControls(const Control *control1, const Control *control2) const
81 {
82  const double *s1 = static_cast<const ControlType*>(control1)->values;
83  const double *s2 = static_cast<const ControlType*>(control2)->values;
84  for (unsigned int i = 0 ; i < dimension_ ; ++i)
85  {
86  double diff = (*s1++) - (*s2++);
87  if (fabs(diff) > std::numeric_limits<double>::epsilon() * 2.0)
88  return false;
89  }
90  return true;
91 }
92 
94 {
96 }
97 
99 {
100  ControlType *rcontrol = new ControlType();
101  rcontrol->values = new double[dimension_];
102  return rcontrol;
103 }
104 
106 {
107  ControlType *rcontrol = static_cast<ControlType*>(control);
108  delete[] rcontrol->values;
109  delete rcontrol;
110 }
111 
113 {
114  ControlType *rcontrol = static_cast<ControlType*>(control);
115  for (unsigned int i = 0 ; i < dimension_ ; ++i)
116  {
117  if (bounds_.low[i] <= 0.0 && bounds_.high[i] >= 0.0)
118  rcontrol->values[i] = 0.0;
119  else
120  rcontrol->values[i] = bounds_.low[i];
121  }
122 }
123 
124 double* ompl::control::RealVectorControlSpace::getValueAddressAtIndex(Control *control, const unsigned int index) const
125 {
126  return index < dimension_ ? static_cast<ControlType*>(control)->values + index : NULL;
127 }
128 
129 void ompl::control::RealVectorControlSpace::printControl(const Control *control, std::ostream &out) const
130 {
131  out << "RealVectorControl [";
132  if (control)
133  {
134  const ControlType *rcontrol = static_cast<const ControlType*>(control);
135  for (unsigned int i = 0 ; i < dimension_ ; ++i)
136  {
137  out << rcontrol->values[i];
138  if (i + 1 < dimension_)
139  out << ' ';
140  }
141  }
142  else
143  out << "NULL";
144  out << ']' << std::endl;
145 }
146 
148 {
149  out << "Real vector control space '" << getName() << "' with bounds: " << std::endl;
150  out << " - min: ";
151  for (unsigned int i = 0 ; i < dimension_ ; ++i)
152  out << bounds_.low[i] << " ";
153  out << std::endl;
154  out << " - max: ";
155  for (unsigned int i = 0 ; i < dimension_ ; ++i)
156  out << bounds_.high[i] << " ";
157  out << std::endl;
158 }
159 
161 {
162  return controlBytes_;
163 }
164 
165 void ompl::control::RealVectorControlSpace::serialize(void *serialization, const Control *ctrl) const
166 {
167  memcpy(serialization, ctrl->as<ControlType>()->values, controlBytes_);
168 }
169 
170 void ompl::control::RealVectorControlSpace::deserialize(Control *ctrl, const void *serialization) const
171 {
172  memcpy(ctrl->as<ControlType>()->values, serialization, controlBytes_);
173 }
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:72
std::vector< double > low
Lower bound.
virtual double * getValueAddressAtIndex(Control *control, const unsigned int index) const
Many controls contain a number of double values. This function provides a means to get the memory add...
Definition of an abstract control.
Definition: Control.h:48
A boost shared pointer wrapper for ompl::control::ControlSampler.
const ControlSpace * space_
The control space this sampler operates on.
virtual void copyControl(Control *destination, const Control *source) const
Copy a control to another.
virtual void printSettings(std::ostream &out) const
Print the settings for this control space to a stream.
virtual bool equalControls(const Control *control1, const Control *control2) const
Check if two controls are the same.
virtual Control * allocControl() const
Allocate memory for a control.
virtual unsigned int getSerializationLength() const
Returns the serialization size for a single control in this space.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
virtual void printControl(const Control *control, std::ostream &out) const
Print a control to a stream.
A control space representing Rn.
std::vector< double > high
Upper bound.
virtual unsigned int getDimension() const
Get the dimension of this control space.
void check() const
Check if the bounds are valid (same length for low and high, high[i] > low[i]). Throw an exception if...
virtual unsigned int getDimension() const =0
Get the dimension of this control space.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:68
RealVectorControlUniformSampler(const ControlSpace *space)
Constructor.
virtual void nullControl(Control *control) const
Make the control have no effect if it were to be applied to a state for any amount of time...
The exception type for ompl.
Definition: Exception.h:47
virtual void freeControl(Control *control) const
Free the memory of a control.
The lower and upper bounds for an Rn space.
virtual void serialize(void *serialization, const Control *ctrl) const
Serializes the given control into the serialization buffer.
virtual void sample(Control *control)
Sample a control. All other control sampling functions default to this one, unless a user-specified i...
RNG rng_
Instance of random number generator.
void setBounds(const base::RealVectorBounds &bounds)
Set the bounds (min max values for each dimension) for the control.
virtual void deserialize(Control *ctrl, const void *serialization) const
Deserializes a control from the serialization buffer.
virtual void setup()
Perform final setup steps. This function is automatically called by the SpaceInformation.
virtual ControlSamplerPtr allocDefaultControlSampler() const
Allocate the default control sampler.
double * values
An array of length n, representing the value of the control.