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
47namespace ob = ompl::base;
48namespace og = ompl::geometric;
52const double pi = boost::math::constants::pi<double>();
53
54// Path Planning in SE3 = R3 \times SO3
55// using quotient-spaces R3 and SE3
56
57bool 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
66bool 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
81bool isStateValid_R3(const ob::State *state)
82{
83 const auto *R3 = state->as<ob::RealVectorStateSpace::StateType>();
84 return isInCollision(R3->values);
85}
86
87int 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}
ompl::base::CompoundState StateType
Define the type of state allocated by this state space.
Definition: StateSpace.h:577
const T * as(unsigned int index) const
Cast a component of this instance to a desired type.
Definition: State.h:95
The lower and upper bounds for an Rn space.
Definition of a scoped state.
Definition: ScopedState.h:57
ompl::base::State StateType
Define the type of state allocated by this space.
Definition: StateSpace.h:78
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
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.
Definition: GeneticSearch.h:48
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49