KoulesProjection.h
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 #ifndef DEMOS_KOULES_PROJECTION_
38 #define DEMOS_KOULES_PROJECTION_
39 
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 
42 // A projection for the KoulesStateSpace
44 {
45 public:
46  KoulesProjection(const ompl::base::StateSpace* space, unsigned int numDimensions = 3)
47  : ompl::base::ProjectionEvaluator(space), numDimensions_(numDimensions)
48  {
49  unsigned int n = (space_->getDimension() - 1) / 2 + 1;
50  if (numDimensions_ > n)
51  numDimensions_ = n;
52  else if (numDimensions_ < 3)
53  numDimensions_ = 3;
54  }
55 
56  virtual unsigned int getDimension() const
57  {
58  return numDimensions_;
59  }
60  virtual void defaultCellSizes()
61  {
62  cellSizes_.resize(numDimensions_, .05);
63  }
64  // projection with coordinates in the same order as described in Andrew
65  // Ladd's thesis: (x,y,theta) of the ship, followed by the positions of
66  // the koules (up to the dimensionality of the projection)
67  virtual void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
68  {
69  const double* x = state->as<ompl::base::RealVectorStateSpace::StateType>()->values;
70  unsigned int numKoules = (numDimensions_ - 3) / 2;
71  projection[0] = x[0];
72  projection[1] = x[1];
73  projection[2] = numDimensions_ == 3 ? distanceGoal(state) : x[2];
74  for (unsigned int i = 0; i < numKoules; ++i)
75  {
76  projection[2 * i + 3] = x[4 * i + 5];
77  projection[2 * i + 4] = x[4 * i + 6];
78  }
79  }
80 
81  // distance to goal is used as a projection coordinate in 3D projections.
82  //
83  // This distance definition is adapted from the KoulesGoal class
84  double distanceGoal(const ompl::base::State *st) const
85  {
86  double minX, minY;
87  const double* v = st->as<KoulesStateSpace::StateType>()->values;
88  std::size_t numKoules = (space_->getDimension() - 5) / 4, liveKoules = numKoules;
89  double minDist = sideLength;
90 
91  for (std::size_t i = 1, j = 5; i <= numKoules; ++i, j += 4)
92  {
93  if (space_->as<KoulesStateSpace>()->isDead(st, i))
94  liveKoules--;
95  else
96  {
97  minX = std::min(v[j ], sideLength - v[j ]);
98  minY = std::min(v[j + 1], sideLength - v[j + 1]);
99  minDist = std::min(minDist, std::min(minX, minY) - kouleRadius);
100  }
101  }
102  if (minDist < 0 || liveKoules == 0)
103  minDist = 0;
104  return .5 * sideLength * (double) liveKoules + minDist;
105  }
106 
107 protected:
108  unsigned int numDimensions_;
109 };
110 
111 #endif
std::vector< double > cellSizes_
The size of a cell, in every dimension of the projected space, in the implicitly defined integer grid...
virtual void project(const ompl::base::State *state, ompl::base::EuclideanProjection &projection) const
Compute the projection as an array of double values.
virtual unsigned int getDimension() const =0
Get the dimension of the space (not the dimension of the surrounding ambient space) ...
virtual unsigned int getDimension() const
Return the dimension of the projection defined by this evaluator.
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
const StateSpace * space_
The state space this projection operates on.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
virtual void defaultCellSizes()
Set the default cell dimensions for this projection. The default implementation of this function is e...
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
boost::numeric::ublas::vector< double > EuclideanProjection
The datatype for state projections. This class contains a real vector.
Definition of an abstract state.
Definition: State.h:49
Abstract definition for a class computing projections to Rn. Implicit integer grids are imposed on th...