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, Eigen::Ref<Eigen::VectorXd> 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 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...
virtual void project(const ompl::base::State *state, Eigen::Ref< Eigen::VectorXd > projection) const
Compute the projection as an array of double values.
Representation of a space in which planning can be performed. Topology specific sampling, interpolation and distance are defined.
Definition: StateSpace.h:70
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...