QuotientSpacePlanningRigidBody3D.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/geometric/planners/quotientspace/QRRT.h>
44 #include <iostream>
45 #include <boost/math/constants/constants.hpp>
46 
47 namespace ob = ompl::base;
48 namespace og = ompl::geometric;
52 const double pi = boost::math::constants::pi<double>();
53 
54 // Path Planning in SE3 = R3 \times SO3
55 // using quotient-spaces R3 and SE3
56 
57 bool isInCollision(double *val)
58 {
59  const double &x = val[0] - 0.5;
60  const double &y = val[1] - 0.5;
61  const double &z = val[2] - 0.5;
62  double d = sqrt(x * x + y * y + z * z);
63  return (d > 0.2);
64 }
65 
66 bool isStateValid_SE3(const ob::State *state)
67 {
68  static auto SO3(std::make_shared<ob::SO3StateSpace>());
69  static SO3State SO3id(SO3);
70  SO3id->setIdentity();
71 
72  const auto *SE3state = state->as<ob::SE3StateSpace::StateType>();
73  const auto *R3state = SE3state->as<ob::RealVectorStateSpace::StateType>(0);
74  const ob::State *SO3state = SE3state->as<ob::SO3StateSpace::StateType>(1);
75  const ob::State *SO3stateIdentity = SO3id->as<ob::SO3StateSpace::StateType>();
76 
77  double d = SO3->distance(SO3state, SO3stateIdentity);
78  return isInCollision(R3state->values) && (d < pi / 4.0);
79 }
80 
81 bool isStateValid_R3(const ob::State *state)
82 {
83  const auto *R3 = state->as<ob::RealVectorStateSpace::StateType>();
84  return isInCollision(R3->values);
85 }
86 
87 int main()
88 {
89  //############################################################################
90  // Step 1: Setup planning problem using several quotient-spaces
91  //############################################################################
92  // Setup SE3
93  auto SE3(std::make_shared<ob::SE3StateSpace>());
94  ob::RealVectorBounds bounds(3);
95  bounds.setLow(0);
96  bounds.setHigh(1);
97  SE3->setBounds(bounds);
98  ob::SpaceInformationPtr si_SE3(std::make_shared<ob::SpaceInformation>(SE3));
99  si_SE3->setStateValidityChecker(isStateValid_SE3);
100 
101  // Setup Quotient-Space R2
102  auto R3(std::make_shared<ob::RealVectorStateSpace>(3));
103  R3->setBounds(0, 1);
104  ob::SpaceInformationPtr si_R3(std::make_shared<ob::SpaceInformation>(R3));
105  si_R3->setStateValidityChecker(isStateValid_R3);
106 
107  // Create vector of spaceinformationptr (last one is original cspace)
108  std::vector<ob::SpaceInformationPtr> si_vec;
109  si_vec.push_back(si_R3);
110  si_vec.push_back(si_SE3);
111 
112  // Define Planning Problem
113  SE3State start_SE3(SE3);
114  SE3State goal_SE3(SE3);
115  start_SE3->setXYZ(0, 0, 0);
116  start_SE3->rotation().setIdentity();
117  goal_SE3->setXYZ(1, 1, 1);
118  goal_SE3->rotation().setIdentity();
119 
120  ob::ProblemDefinitionPtr pdef = std::make_shared<ob::ProblemDefinition>(si_SE3);
121  pdef->setStartAndGoalStates(start_SE3, goal_SE3);
122 
123  //############################################################################
124  // Step 2: Do path planning as usual but with a sequence of
125  // spaceinformationptr
126  //############################################################################
127  auto planner = std::make_shared<og::QRRT>(si_vec);
128 
129  // Planner can be used as any other OMPL algorithm
130  planner->setProblemDefinition(pdef);
131  planner->setup();
132 
133  ob::PlannerStatus solved = planner->ob::Planner::solve(1.0);
134 
135  if (solved)
136  {
137  std::cout << std::string(80, '-') << std::endl;
138  std::cout << "Configuration-Space Path (SE3):" << std::endl;
139  std::cout << std::string(80, '-') << std::endl;
140  pdef->getSolutionPath()->print(std::cout);
141 
142  std::cout << std::string(80, '-') << std::endl;
143  std::cout << "Quotient-Space Path (R3):" << std::endl;
144  std::cout << std::string(80, '-') << std::endl;
145  const ob::ProblemDefinitionPtr pdefR3 = planner->getProblemDefinition(0);
146  pdefR3->getSolutionPath()->print(std::cout);
147 
148  std::vector<int> nodes = planner->getFeasibleNodes();
149 
150  std::cout << std::string(80, '-') << std::endl;
151  for (unsigned int k = 0; k < nodes.size(); k++)
152  {
153  std::cout << "QuotientSpace" << k << " has " << nodes.at(k) << " nodes." << std::endl;
154  }
155  }
156  return 0;
157 }
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:80
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:159
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.