QuotientSpacePlanningRigidBody2D.cpp
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2019, University of Stuttgart
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 University of Stuttgart nor the names
18  * of its contributors may be used to endorse or promote products
19  * derived from this software without specific prior written
20  * permission.
21  *
22  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
23  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
24  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
25  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
26  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
27  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
28  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
29  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
30  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
31  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
32  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
33  * POSSIBILITY OF SUCH DAMAGE.
34  *********************************************************************/
35 
36 /* Author: Andreas Orthey */
37 
38 #include <ompl/base/spaces/SE2StateSpace.h>
39 #include <ompl/base/spaces/RealVectorStateSpace.h>
40 #include <ompl/base/SpaceInformation.h>
41 #include <ompl/base/StateSpace.h>
42 #include <ompl/geometric/planners/quotientspace/QRRT.h>
43 #include <iostream>
44 #include <boost/math/constants/constants.hpp>
45 
46 namespace ob = ompl::base;
47 namespace og = ompl::geometric;
48 
49 // Path Planning in SE2 = R2 \times SO2
50 // using quotient-spaces R2 and SE2
51 
52 bool boxConstraint(const double values[])
53 {
54  const double &x = values[0] - 0.5;
55  const double &y = values[1] - 0.5;
56  double pos_cnstr = sqrt(x * x + y * y);
57  return (pos_cnstr > 0.2);
58 }
59 bool isStateValid_SE2(const ob::State *state)
60 {
61  const auto *SE2state = state->as<ob::SE2StateSpace::StateType>();
62  const auto *R2 = SE2state->as<ob::RealVectorStateSpace::StateType>(0);
63  const auto *SO2 = SE2state->as<ob::SO2StateSpace::StateType>(1);
64  return boxConstraint(R2->values) && (SO2->value < boost::math::constants::pi<double>() / 2.0);
65 }
66 bool isStateValid_R2(const ob::State *state)
67 {
68  const auto *R2 = state->as<ob::RealVectorStateSpace::StateType>();
69  return boxConstraint(R2->values);
70 }
71 
72 int main()
73 {
74  // Setup SE2
75  auto SE2(std::make_shared<ob::SE2StateSpace>());
76  ob::RealVectorBounds bounds(2);
77  bounds.setLow(0);
78  bounds.setHigh(1);
79  SE2->setBounds(bounds);
80  ob::SpaceInformationPtr si_SE2(std::make_shared<ob::SpaceInformation>(SE2));
81  si_SE2->setStateValidityChecker(isStateValid_SE2);
82 
83  // Setup Quotient-Space R2
84  auto R2(std::make_shared<ob::RealVectorStateSpace>(2));
85  R2->setBounds(0, 1);
86  ob::SpaceInformationPtr si_R2(std::make_shared<ob::SpaceInformation>(R2));
87  si_R2->setStateValidityChecker(isStateValid_R2);
88 
89  // Create vector of spaceinformationptr
90  std::vector<ob::SpaceInformationPtr> si_vec;
91  si_vec.push_back(si_R2);
92  si_vec.push_back(si_SE2);
93 
94  // Define Planning Problem
95  using SE2State = ob::ScopedState<ob::SE2StateSpace>;
96  SE2State start_SE2(SE2);
97  SE2State goal_SE2(SE2);
98  start_SE2->setXY(0, 0);
99  start_SE2->setYaw(0);
100  goal_SE2->setXY(1, 1);
101  goal_SE2->setYaw(0);
102 
103  ob::ProblemDefinitionPtr pdef = std::make_shared<ob::ProblemDefinition>(si_SE2);
104  pdef->setStartAndGoalStates(start_SE2, goal_SE2);
105 
106  // Setup Planner using vector of spaceinformationptr
107  auto planner = std::make_shared<og::QRRT>(si_vec);
108 
109  // Planner can be used as any other OMPL algorithm
110  planner->setProblemDefinition(pdef);
111  planner->setup();
112 
113  ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);
114 
115  if (solved)
116  {
117  std::cout << std::string(80, '-') << std::endl;
118  std::cout << "Configuration-Space Path (SE2):" << std::endl;
119  std::cout << std::string(80, '-') << std::endl;
120  pdef->getSolutionPath()->print(std::cout);
121 
122  std::cout << std::string(80, '-') << std::endl;
123  std::cout << "Quotient-Space Path (R2):" << std::endl;
124  std::cout << std::string(80, '-') << std::endl;
125  const ob::ProblemDefinitionPtr pdefR2 = planner->getProblemDefinition(0);
126  pdefR2->getSolutionPath()->print(std::cout);
127 
128  std::vector<int> nodes = planner->getFeasibleNodes();
129  std::cout << std::string(80, '-') << std::endl;
130  for (unsigned int k = 0; k < nodes.size(); k++)
131  {
132  std::cout << "QuotientSpace" << k << " has " << nodes.at(k) << " nodes." << std::endl;
133  }
134  }
135  return 0;
136 }
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:80
Definition of an abstract state.
Definition: State.h:114
This namespace contains sampling based planning routines shared by both planning under geometric cons...
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:142
A class to store the exit status of Planner::solve()
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:641
Definition of a scoped state.
Definition: ScopedState.h:121
The lower and upper bounds for an Rn space.