KoulesSetup.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 "KoulesSetup.h"
38 #include "KoulesGoal.h"
39 #include "KoulesStateSpace.h"
40 #include "KoulesControlSpace.h"
41 #include "KoulesStatePropagator.h"
42 #include "KoulesDirectedControlSampler.h"
43 #include "KoulesDecomposition.h"
44 #include <ompl/base/spaces/RealVectorStateSpace.h>
45 #include <ompl/control/planners/rrt/RRT.h>
46 #include <ompl/control/planners/kpiece/KPIECE1.h>
47 #include <ompl/control/planners/est/EST.h>
48 #include <ompl/control/planners/pdst/PDST.h>
49 #include <ompl/control/planners/sst/SST.h>
50 #include <ompl/control/planners/syclop/SyclopRRT.h>
51 #include <ompl/control/planners/syclop/SyclopEST.h>
52 
53 
54 namespace ob = ompl::base;
55 namespace oc = ompl::control;
56 
58 class KoulesStateValidityChecker : public ompl::base::StateValidityChecker
59 {
60 public:
61  KoulesStateValidityChecker(const ompl::base::SpaceInformationPtr &si) : ompl::base::StateValidityChecker(si)
62  {
63  }
64 
65  bool isValid(const ob::State *state) const override
66  {
67  return si_->satisfiesBounds(state);
68  }
69 };
71 
72 ompl::control::DirectedControlSamplerPtr KoulesDirectedControlSamplerAllocator(
73  const ompl::control::SpaceInformation *si, const ompl::base::GoalPtr &goal, bool propagateMax)
74 {
75  return std::make_shared<KoulesDirectedControlSampler>(si, goal, propagateMax);
76 }
77 
78 
79 KoulesSetup::KoulesSetup(unsigned int numKoules, const std::string& plannerName,
80  const std::vector<double>& stateVec)
81  : ompl::control::SimpleSetup(std::make_shared<KoulesControlSpace>(numKoules))
82 {
83  initialize(numKoules, plannerName, stateVec);
84 }
85 
86 KoulesSetup::KoulesSetup(unsigned int numKoules, const std::string& plannerName, double kouleVel)
87  : ompl::control::SimpleSetup(std::make_shared<KoulesControlSpace>(numKoules))
88 {
89  initialize(numKoules, plannerName);
90  double* state = getProblemDefinition()->getStartState(0)->as<KoulesStateSpace::StateType>()->values;
91  double theta;
92  ompl::RNG rng;
93  for (unsigned int i = 0; i < numKoules; ++i)
94  {
95  theta = rng.uniformReal(0., 2. * boost::math::constants::pi<double>());
96  state[4 * i + 7] = kouleVel * cos(theta);
97  state[4 * i + 8] = kouleVel * sin(theta);
98  }
99 }
100 
101 void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
102  const std::vector<double>& stateVec)
103 {
104  const ob::StateSpacePtr& space = getStateSpace();
105  space->setup();
106  // setup start state
107  ob::ScopedState<> start(space);
108  if (stateVec.size() == space->getDimension())
109  space->copyFromReals(start.get(), stateVec);
110  else
111  {
112  // Pick koule positions evenly radially distributed, but at a linearly
113  // increasing distance from the center. The ship's initial position is
114  // at the center. Initial velocities are 0.
115  std::vector<double> startVec(space->getDimension(), 0.);
116  double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
117  startVec[0] = .5 * sideLength;
118  startVec[1] = .5 * sideLength;
119  startVec[4] = .5 * delta;
120  for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
121  {
122  r = .1 + i * .1 / numKoules;
123  startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
124  startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
125  }
126  space->copyFromReals(start.get(), startVec);
127  }
128  setStartState(start);
129  // set goal
130  setGoal(std::make_shared<KoulesGoal>(si_));
131  // set propagation step size
132  si_->setPropagationStepSize(propagationStepSize);
133  // set min/max propagation steps
134  si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
135  // set directed control sampler; when using the PDST planner, propagate as long as possible
136  bool isPDST = plannerName == "pdst";
137  const ompl::base::GoalPtr& goal = getGoal();
138  si_->setDirectedControlSamplerAllocator(
139  [&goal, isPDST](const ompl::control::SpaceInformation *si)
140  {
141  return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
142  });
143  // set planner
144  setPlanner(getConfiguredPlannerInstance(plannerName));
145  // set validity checker
146  setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
147  // set state propagator
148  setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
149 }
150 
151 ob::PlannerPtr KoulesSetup::getConfiguredPlannerInstance(const std::string& plannerName)
152 {
153  if (plannerName == "rrt")
154  {
155  auto rrtplanner(std::make_shared<oc::RRT>(si_));
156  rrtplanner->setIntermediateStates(true);
157  return rrtplanner;
158  }
159  if (plannerName == "est")
160  return std::make_shared<oc::EST>(si_);
161  if (plannerName == "kpiece")
162  return std::make_shared<oc::KPIECE1>(si_);
163  if (plannerName == "sst")
164  {
165  auto sstplanner(std::make_shared<oc::SST>(si_));
166  sstplanner->setSelectionRadius(0.05);
167  sstplanner->setPruningRadius(0.001);
168  return sstplanner;
169  }
170  if (plannerName == "sycloprrt")
171  return std::make_shared<oc::SyclopRRT>(si_, std::make_shared<KoulesDecomposition>(getStateSpace()));
172  if (plannerName == "syclopest")
173  return std::make_shared<oc::SyclopEST>(si_, std::make_shared<KoulesDecomposition>(getStateSpace()));
174 
175  auto pdstplanner(std::make_shared<oc::PDST>(si_));
176  pdstplanner->setProjectionEvaluator(
177  si_->getStateSpace()->getProjection("PDSTProjection"));
178  return pdstplanner;
179 }
Definition of a scoped state.
Definition: ScopedState.h:56
This namespace contains sampling based planning routines used by planning under differential constrai...
Definition: Control.h:44
A shared pointer wrapper for ompl::base::StateSpace.
STL namespace.
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
A shared pointer wrapper for ompl::base::Planner.
Random number generation. An instance of this class cannot be used by multiple threads at once (membe...
Definition: RandomNumbers.h:56
A shared pointer wrapper for ompl::control::DirectedControlSampler.
Abstract definition for a class checking the validity of states. The implementation of this class mus...
double uniformReal(double lower_bound, double upper_bound)
Generate a random real within given bounds: [lower_bound, upper_bound)
Definition: RandomNumbers.h:72
A shared pointer wrapper for ompl::base::SpaceInformation.
Definition of an abstract state.
Definition: State.h:49
This namespace contains sampling based planning routines shared by both planning under geometric cons...
virtual bool isValid(const State *state) const =0
Return true if the state state is valid. Usually, this means at least collision checking. If it is possible that ompl::base::StateSpace::interpolate() or ompl::control::ControlSpace::propagate() return states that are outside of bounds, this function should also make a call to ompl::base::SpaceInformation::satisfiesBounds().
A shared pointer wrapper for ompl::base::Goal.
Space information containing necessary information for planning with controls. setup() needs to be ca...