Loading...
Searching...
No Matches
SpaceTimePlanning.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2021, Technische Universität Berlin (TU Berlin)
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 TU Berlin 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: Francesco Grothe */
36
37#include <ompl/base/SpaceInformation.h>
38#include <ompl/base/spaces/SpaceTimeStateSpace.h>
39#include <ompl/geometric/planners/rrt/RRTConnect.h>
40#include <ompl/geometric/planners/rrt/STRRTstar.h>
41#include <ompl/geometric/SimpleSetup.h>
42
43#include <ompl/config.h>
44#include <iostream>
45
46namespace ob = ompl::base;
47namespace og = ompl::geometric;
48
52
53bool isStateValid(const ob::State *state)
54{
55 // extract the space component of the state and cast it to what we expect
56 const auto pos = state->as<ob::CompoundState>()->as<ob::RealVectorStateSpace::StateType>(0)->values[0];
57
58 // extract the time component of the state and cast it to what we expect
59 const auto t = state->as<ob::CompoundState>()->as<ob::TimeStateSpace::StateType>(1)->position;
60
61 // check validity of state defined by pos & t (e.g. check if constraints are satisfied)...
62
63 // return a value that is always true
64 return t >= 0 && pos < std::numeric_limits<double>::infinity();
65}
66
67class SpaceTimeMotionValidator : public ob::MotionValidator {
68
69public:
70 explicit SpaceTimeMotionValidator(const ob::SpaceInformationPtr &si) : MotionValidator(si),
71 vMax_(si_->getStateSpace().get()->as<ob::SpaceTimeStateSpace>()->getVMax()),
72 stateSpace_(si_->getStateSpace().get()) {};
73
74 bool checkMotion(const ob::State *s1, const ob::State *s2) const override
75 {
76 // assume motion starts in a valid configuration, so s1 is valid
77 if (!si_->isValid(s2)) {
78 invalid_++;
79 return false;
80 }
81
82 // check if motion is forward in time and is not exceeding the speed limit
83 auto *space = stateSpace_->as<ob::SpaceTimeStateSpace>();
84 auto deltaPos = space->distanceSpace(s1, s2);
85 auto deltaT = s2->as<ob::CompoundState>()->as<ob::TimeStateSpace::StateType>(1)->position -
86 s1->as<ob::CompoundState>()->as<ob::TimeStateSpace::StateType>(1)->position;
87
88 if (!(deltaT > 0 && deltaPos / deltaT <= vMax_)) {
89 invalid_++;
90 return false;
91 }
92
93 // check if the path between the states is unconstrained (perform interpolation)...
94
95 return true;
96 }
97
98 bool checkMotion(const ompl::base::State *, const ompl::base::State *,
99 std::pair<ob::State *, double> &) const override
100 {
101 throw ompl::Exception("SpaceTimeMotionValidator::checkMotion", "not implemented");
102 }
103
104private:
105 double vMax_; // maximum velocity
106 ob::StateSpace *stateSpace_; // the animation state space for distance calculation
107};
108
109void plan(void)
110{
111 // set maximum velocity
112 double vMax = 0.2;
113
114 // construct the state space we are planning in
115 auto vectorSpace(std::make_shared<ob::RealVectorStateSpace>(1));
116 auto space = std::make_shared<ob::SpaceTimeStateSpace>(vectorSpace, vMax);
117
118 // set the bounds for R1
119 ob::RealVectorBounds bounds(1);
120 bounds.setLow(-1.0);
121 bounds.setHigh(1.0);
122 vectorSpace->setBounds(bounds);
123
124 // set time bounds. Planning with unbounded time is also possible when using ST-RRT*.
125 space->setTimeBounds(0.0, 10.0);
126
127 // create the space information class for the space
128 ob::SpaceInformationPtr si = std::make_shared<ob::SpaceInformation>(space);
129
130 // set state validity checking for this space
131 si->setStateValidityChecker([](const ob::State *state) { return isStateValid(state); });
132 si->setMotionValidator(std::make_shared<SpaceTimeMotionValidator>(si));
133
134 // define a simple setup class
135 og::SimpleSetup ss(si);
136
137 // create a start state
138 ob::ScopedState<> start(space);
139 start[0] = 0; // pos
140
141 // create a goal state
142 ob::ScopedState<> goal(space);
143 goal[0] = 1; // pos
144
145 // set the start and goal states
146 ss.setStartAndGoalStates(start, goal);
147
148 // construct the planner
149 auto *strrtStar = new og::STRRTstar(si);
150
151 // set planner parameters
152 strrtStar->setRange(vMax);
153
154 // set the used planner
155 ss.setPlanner(ob::PlannerPtr(strrtStar));
156
157 // attempt to solve the problem within one second of planning time
158 ob::PlannerStatus solved = ss.solve(1.0);
159
160 if (solved)
161 {
162 std::cout << "Found solution:" << std::endl;
163 // print the path to screen
164 ss.getSolutionPath().print(std::cout);
165 }
166 else
167 std::cout << "No solution found" << std::endl;
168
169}
170
171int main(int /*argc*/, char ** /*argv*/)
172{
173 std::cout << "OMPL version: " << OMPL_VERSION << std::endl;
174
175 plan();
176
177 return 0;
178}
Definition of a compound state.
Definition State.h:87
Abstract definition for a class checking the validity of motions – path segments between states....
MotionValidator(SpaceInformation *si)
Constructor.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
unsigned int invalid_
Number of invalid segments.
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition ScopedState.h:57
Definition of an abstract state.
Definition State.h:50
const T * as() const
Cast this instance to a desired type.
Definition State.h:66
Space-Time RRT* (STRRTstar)
Definition STRRTstar.h:66
Create the set of classes typically needed to solve a geometric problem.
Definition SimpleSetup.h:63
This namespace contains sampling based planning routines shared by both planning under geometric cons...
This namespace contains code that is specific to planning under geometric constraints.
A class to store the exit status of Planner::solve()