KoulesControlSpace.cpp
1 /*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2013, 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: Beck Chen, Mark Moll */
36 
37 #include "KoulesConfig.h"
38 #include "KoulesStateSpace.h"
39 #include "KoulesControlSpace.h"
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 
42 namespace ob = ompl::base;
43 namespace oc = ompl::control;
44 
46 {
47  const ob::RealVectorBounds &bounds = space_->as<oc::RealVectorControlSpace>()->getBounds();
48  oc::RealVectorControlSpace::ControlType *rcontrol =
49  control->as<oc::RealVectorControlSpace::ControlType>();
50  double r = rng_.uniformReal(bounds.low[0], bounds.high[0]);
51  double theta = rng_.uniformReal(0., 2. * boost::math::constants::pi<double>());
52  rcontrol->values[0] = r * cos(theta);
53  rcontrol->values[1] = r * sin(theta);
54 }
55 
57 {
58  steer(control, state, rng_.uniformReal(0., sideLength), rng_.uniformReal(0., sideLength));
59 }
60 
61 void KoulesControlSampler::steer(oc::Control *control, const ob::State *state, double x, double y)
62 {
63  const double* r = state->as<KoulesStateSpace::StateType>()->values;
64  double dx = x - r[0];
65  double dy = y - r[1];
66  double xNrm2 = dx * dx + dy * dy;
67  if (xNrm2 > std::numeric_limits<float>::epsilon())
68  {
69  const ob::RealVectorBounds &bounds = space_->as<oc::RealVectorControlSpace>()->getBounds();
70  double v = rng_.uniformReal(bounds.low[0], bounds.high[0]) / sqrt(xNrm2);
71  oc::RealVectorControlSpace::ControlType *rcontrol =
72  control->as<oc::RealVectorControlSpace::ControlType>();
73  rcontrol->values[0] = v * dx;
74  rcontrol->values[1] = v * dy;
75  }
76  else
77  sample(control);
78 }
79 
80 KoulesControlSpace::KoulesControlSpace(unsigned int numKoules)
82  std::make_shared<KoulesStateSpace>(numKoules), 2)
83 {
84  bounds_.setLow(shipVmin);
85  bounds_.setHigh(shipVmax);
86 }
87 
const T * as() const
Cast this instance to a desired type.
Definition: Control.h:64
std::vector< double > low
Lower bound.
Definition of an abstract control.
Definition: Control.h:47
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
const ControlSpace * space_
The control space this sampler operates on.
T * as()
Cast this instance to a desired type.
Definition: ControlSpace.h:77
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
A control space representing Rn.
std::vector< double > high
Upper bound.
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:74
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
Definition: Cost.h:44
The lower and upper bounds for an Rn space.
virtual void sample(ompl::control::Control *control)
Sample a control. All other control sampling functions default to this one, unless a user-specified i...