KoulesStateSpace.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 "KoulesProjection.h"
40 
41 namespace ob = ompl::base;
42 
43 KoulesStateSpace::KoulesStateSpace(unsigned int numKoules)
44  : RealVectorStateSpace(4 * numKoules + 5), mass_(numKoules + 1, kouleMass),
45  radius_(numKoules + 1, kouleRadius)
46 {
47  mass_[0] = shipMass;
48  radius_[0] = shipRadius;
49  setName("Koules" + std::to_string(numKoules) + getName());
50  // layout: (x_s y_s vx_s vy_s theta_s ... x_i y_i vx_i vy_i ... ),
51  // where (x_i, y_i) is the position of koule i (i=1,..,numKoules),
52  // (vx_i, vy_i) its velocity, (x_s, y_s) the position of the ship,
53  // (vx_s, vy_s) its velocity, and theta_s its orientation.
54 
55  // create the bounds
56  unsigned int j = 0;
57  // set the bounds for the ship's position
58  bounds_.setLow(j, shipRadius);
59  bounds_.setHigh(j++, sideLength - shipRadius);
60  bounds_.setLow(j, shipRadius);
61  bounds_.setHigh(j++, sideLength - shipRadius);
62  // set the bounds for the ship's velocity
63  bounds_.setLow(j, -10.);
64  bounds_.setHigh(j++, 10.);
65  bounds_.setLow(j, -10.);
66  bounds_.setHigh(j++, 10.);
67  // set bounds on orientation
68  bounds_.setLow(j, -boost::math::constants::pi<double>());
69  bounds_.setHigh(j++, boost::math::constants::pi<double>());
70  for (unsigned int i = 0; i < numKoules; ++i)
71  {
72  // set the bounds for koule i's position
73  bounds_.setLow(j, -2. * kouleRadius);
74  bounds_.setHigh(j++, sideLength + 2. * kouleRadius);
75  bounds_.setLow(j, -2. * kouleRadius);
76  bounds_.setHigh(j++, sideLength + 2. * kouleRadius);
77  // set the bounds for koule i's velocity
78  bounds_.setLow(j, -10);
79  bounds_.setHigh(j++, 10.);
80  bounds_.setLow(j, -10.);
81  bounds_.setHigh(j++, 10.);
82  }
83 }
84 
85 void KoulesStateSpace::registerProjections()
86 {
87  registerDefaultProjection(std::make_shared<KoulesProjection>(this, (getDimension() - 1) / 4 + 1));
88  registerProjection("PDSTProjection", std::make_shared<KoulesProjection>(this, (getDimension() - 1) / 2 + 1));
89 }
90 
91 bool KoulesStateSpace::isDead(const ompl::base::State* state, unsigned int i) const
92 {
93  const StateType* s = static_cast<const StateType*>(state);
94  return s->values[i ? 4 * i + 1 : 0] == -2. * kouleRadius;
95 }
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