MultiLevelPlanningRigidBody3D.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/SE3StateSpace.h>
39 #include <ompl/base/spaces/SO3StateSpace.h>
40 #include <ompl/base/spaces/RealVectorStateSpace.h>
41 #include <ompl/base/SpaceInformation.h>
42 #include <ompl/base/StateSpace.h>
43 #include <ompl/multilevel/planners/qrrt/QRRT.h>
44 #include <iostream>
45 #include <boost/math/constants/constants.hpp>
46 
47 using namespace ompl::base;
48 
52 const double pi = boost::math::constants::pi<double>();
53 
54 // Path Planning on fiber bundle SE3 \rightarrow R3
55 
56 bool isInCollision(double *val)
57 {
58  const double &x = val[0] - 0.5;
59  const double &y = val[1] - 0.5;
60  const double &z = val[2] - 0.5;
61  double d = sqrt(x * x + y * y + z * z);
62  return (d > 0.2);
63 }
64 
65 bool isStateValid_SE3(const State *state)
66 {
67  static auto SO3(std::make_shared<SO3StateSpace>());
68  static SO3State SO3id(SO3);
69  SO3id->setIdentity();
70 
71  const auto *SE3state = state->as<SE3StateSpace::StateType>();
72  const auto *R3state = SE3state->as<RealVectorStateSpace::StateType>(0);
73  const State *SO3state = SE3state->as<SO3StateSpace::StateType>(1);
74  const State *SO3stateIdentity = SO3id->as<SO3StateSpace::StateType>();
75 
76  double d = SO3->distance(SO3state, SO3stateIdentity);
77  return isInCollision(R3state->values) && (d < pi / 4.0);
78 }
79 
80 bool isStateValid_R3(const State *state)
81 {
82  const auto *R3 = state->as<RealVectorStateSpace::StateType>();
83  return isInCollision(R3->values);
84 }
85 
86 int main()
87 {
88  //############################################################################
89  // Step 1: Setup planning problem using several quotient-spaces
90  //############################################################################
91  // Setup SE3
92  auto SE3(std::make_shared<SE3StateSpace>());
93  RealVectorBounds bounds(3);
94  bounds.setLow(0);
95  bounds.setHigh(1);
96  SE3->setBounds(bounds);
97  SpaceInformationPtr si_SE3(std::make_shared<SpaceInformation>(SE3));
98  si_SE3->setStateValidityChecker(isStateValid_SE3);
99 
100  // Setup Quotient-Space R2
101  auto R3(std::make_shared<RealVectorStateSpace>(3));
102  R3->setBounds(0, 1);
103  SpaceInformationPtr si_R3(std::make_shared<SpaceInformation>(R3));
104  si_R3->setStateValidityChecker(isStateValid_R3);
105 
106  // Create vector of spaceinformationptr (last one is original cspace)
107  std::vector<SpaceInformationPtr> si_vec;
108  si_vec.push_back(si_R3);
109  si_vec.push_back(si_SE3);
110 
111  // Define Planning Problem
112  SE3State start_SE3(SE3);
113  SE3State goal_SE3(SE3);
114  start_SE3->setXYZ(0, 0, 0);
115  start_SE3->rotation().setIdentity();
116  goal_SE3->setXYZ(1, 1, 1);
117  goal_SE3->rotation().setIdentity();
118 
119  ProblemDefinitionPtr pdef = std::make_shared<ProblemDefinition>(si_SE3);
120  pdef->setStartAndGoalStates(start_SE3, goal_SE3);
121 
122  //############################################################################
123  // Step 2: Do path planning as usual but with a sequence of
124  // spaceinformationptr
125  //############################################################################
126  auto planner = std::make_shared<ompl::multilevel::QRRT>(si_vec);
127 
128  // Planner can be used as any other OMPL algorithm
129  planner->setProblemDefinition(pdef);
130  planner->setup();
131 
132  PlannerStatus solved = planner->Planner::solve(1.0);
133 
134  if (solved)
135  {
136  std::cout << std::string(80, '-') << std::endl;
137  std::cout << "Bundle-Space Path (SE3):" << std::endl;
138  std::cout << std::string(80, '-') << std::endl;
139  pdef->getSolutionPath()->print(std::cout);
140 
141  std::cout << std::string(80, '-') << std::endl;
142  std::cout << "Base-Space Path (R3) :" << std::endl;
143  std::cout << std::string(80, '-') << std::endl;
144  const ProblemDefinitionPtr pdefR3 = planner->getProblemDefinition(0);
145  pdefR3->getSolutionPath()->print(std::cout);
146  }
147  return 0;
148 }
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
A shared pointer wrapper for ompl::control::SpaceInformation.
Definition of an abstract state.
Definition: State.h:113
This namespace contains sampling based planning routines shared by both planning under geometric cons...
The definition of a state in SO(3) represented as a unit quaternion.
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
A class to store the exit status of Planner::solve()
A shared pointer wrapper for ompl::base::ProblemDefinition.
Definition of a scoped state.
Definition: ScopedState.h:120
The lower and upper bounds for an Rn space.