Loading...
Searching...
No Matches
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
47using namespace ompl::base;
48
52const double pi = boost::math::constants::pi<double>();
53
54// Path Planning on fiber bundle SE3 \rightarrow R3
55
56bool 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
65bool 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
80bool isStateValid_R3(const State *state)
81{
82 const auto *R3 = state->as<RealVectorStateSpace::StateType>();
83 return isInCollision(R3->values);
84}
85
86int 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:95
A shared pointer wrapper for ompl::base::ProblemDefinition.
The lower and upper bounds for an Rn space.
A state in SE(3): position = (x, y, z), quaternion = (x, y, z, w)
The definition of a state in SO(3) represented as a unit quaternion.
Definition of a scoped state.
Definition ScopedState.h:57
A shared pointer wrapper for ompl::base::SpaceInformation.
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...
A class to store the exit status of Planner::solve()