Loading...
Searching...
No Matches
MultiLevelPlanarManipulatorDemo.cpp
1/*********************************************************************
2 * Software License Agreement (BSD License)
3 *
4 * Copyright (c) 2021,
5 * Max Planck Institute for Intelligent Systems (MPI-IS).
6 * All rights reserved.
7 *
8 * Redistribution and use in source and binary forms, with or without
9 * modification, are permitted provided that the following conditions
10 * are met:
11 *
12 * * Redistributions of source code must retain the above copyright
13 * notice, this list of conditions and the following disclaimer.
14 * * Redistributions in binary form must reproduce the above
15 * copyright notice, this list of conditions and the following
16 * disclaimer in the documentation and/or other materials provided
17 * with the distribution.
18 * * Neither the name of the MPI-IS nor the names
19 * of its contributors may be used to endorse or promote products
20 * derived from this software without specific prior written
21 * permission.
22 *
23 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
24 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
25 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
26 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
27 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
28 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
29 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
30 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
31 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
32 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
33 * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
34 * POSSIBILITY OF SUCH DAMAGE.
35 *********************************************************************/
36
37/* Author: Andreas Orthey */
38
39// This is basically just a simplified version of Ryan Luna's Demo, used for
40// testing purposes of the multilevel planning framework
41
42#include <fstream>
43
44#include "boost/program_options.hpp"
45#include "../PlanarManipulator/PolyWorld.h"
46#include "../PlanarManipulator/PlanarManipulatorPolyWorld.h"
47#include "../PlanarManipulator/PlanarManipulator.h"
48#include "../PlanarManipulator/PlanarManipulatorStateSpace.h"
49#include "../PlanarManipulator/PlanarManipulatorStateValidityChecker.h"
50#include "../PlanarManipulator/PlanarManipulatorIKGoal.h"
51#include "MultiLevelPlanarManipulatorCommon.h"
52
53#include <ompl/geometric/planners/rrt/RRTConnect.h>
54#include <ompl/geometric/planners/rrt/RRT.h>
55#include <ompl/base/spaces/SE2StateSpace.h>
56#include <ompl/base/spaces/RealVectorStateSpace.h>
57#include <ompl/multilevel/planners/qrrt/QRRT.h>
58#include <ompl/multilevel/planners/qmp/QMP.h>
59
60#include <ompl/multilevel/datastructures/Projection.h>
61#include <ompl/multilevel/datastructures/projections/SE2_R2.h>
62
63using namespace ompl::geometric;
64
65namespace ompl
66{
67 namespace multilevel
68 {
69 OMPL_CLASS_FORWARD(Projection);
70 }
71}
72
73class ProjectionJointSpaceToSE2 : public ompl::multilevel::Projection
74{
75public:
76 ProjectionJointSpaceToSE2(StateSpacePtr bundle, StateSpacePtr base, PlanarManipulator *manip)
77 : Projection(bundle, base), manip_(manip)
78 {
80 }
81
82 void project(const State *xBundle, State *xBase) const
83 {
84 std::vector<double> reals;
85 getBundle()->copyToReals(reals, xBundle);
86
87 Eigen::Affine2d eeFrame;
88 manip_->FK(reals, eeFrame);
89
90 double x = eeFrame.translation()(0);
91 double y = eeFrame.translation()(1);
92 double yaw = acos(eeFrame.matrix()(0, 0));
93
94 xBase->as<SE2StateSpace::StateType>()->setXY(x, y);
95 xBase->as<SE2StateSpace::StateType>()->setYaw(yaw);
96
97 getBundle()->printState(xBundle);
98 getBase()->printState(xBase);
99 }
100
101 void lift(const State *xBase, State *xBundle) const
102 {
103 std::vector<double> reals;
104 getBase()->copyToReals(reals, xBase);
105
106 // to Eigen
107 Eigen::Affine2d eeFrame;
108 eeFrame.translation()(0) = reals.at(0);
109 eeFrame.translation()(1) = reals.at(1);
110 eeFrame.rotate(reals.at(2));
111
112 std::vector<double> solution;
113 manip_->FABRIK(solution, eeFrame);
114
115 double *angles = xBundle->as<PlanarManipulatorStateSpace::StateType>()->values;
116 for (uint k = 0; k < solution.size(); k++)
117 {
118 angles[k] = solution.at(k);
119 }
120 }
121
122private:
123 PlanarManipulator *manip_;
124};
125
126int main()
127{
128 Eigen::Affine2d baseFrame;
129 Eigen::Affine2d goalFrame;
130
131 PlanarManipulator manipulator = PlanarManipulator(numLinks, 1.0 / numLinks);
132 PolyWorld world = createCorridorProblem(numLinks, baseFrame, goalFrame);
133
134 //#########################################################################
135 //## Create robot joint configuration space [TOTAL SPACE]
136 //#########################################################################
137 ompl::base::StateSpacePtr space(new PlanarManipulatorStateSpace(numLinks));
138 ompl::base::RealVectorBounds bounds(numLinks);
139 bounds.setLow(-M_PI);
140 bounds.setHigh(M_PI);
141 space->as<PlanarManipulatorStateSpace>()->setBounds(bounds);
142 manipulator.setBounds(bounds.low, bounds.high);
143
144 SpaceInformationPtr si = std::make_shared<SpaceInformation>(space);
145 si->setStateValidityChecker(std::make_shared<PlanarManipulatorCollisionChecker>(si, manipulator, &world));
146 si->setStateValidityCheckingResolution(0.001);
147
148 //#########################################################################
149 //## Create task space [SE2 BASE SPACE]
150 //#########################################################################
151 ompl::base::StateSpacePtr spaceSE2(new SE2StateSpace());
152 ompl::base::RealVectorBounds boundsWorkspace(2);
153 boundsWorkspace.setLow(-2);
154 boundsWorkspace.setHigh(+2);
155 spaceSE2->as<SE2StateSpace>()->setBounds(boundsWorkspace);
156
157 SpaceInformationPtr siSE2 = std::make_shared<SpaceInformation>(spaceSE2);
158 siSE2->setStateValidityChecker(std::make_shared<SE2CollisionChecker>(siSE2, &world));
159 siSE2->setStateValidityCheckingResolution(0.001);
160
161 //#########################################################################
162 //## Create task space [R2 BASE SPACE]
163 //#########################################################################
164 // ompl::base::StateSpacePtr spaceR2(new RealVectorStateSpace(2));
165 // ompl::base::RealVectorBounds boundsR2(2);
166 // boundsR2.setLow(-2);
167 // boundsR2.setHigh(+2);
168 // spaceR2->as<RealVectorStateSpace>()->setBounds(boundsR2);
169
170 // SpaceInformationPtr siR2 = std::make_shared<SpaceInformation>(spaceR2);
171 // // siR2->setStateValidityChecker(std::make_shared<AllValidStateValidityChecker>(siR2));
172 // siR2->setStateValidityChecker(std::make_shared<R2CollisionChecker>(siR2, &world));
173 // siR2->setStateValidityCheckingResolution(0.001);
174
175 //#########################################################################
176 //## Create mapping total to base space [PROJECTION]
177 //#########################################################################
178 ompl::multilevel::ProjectionPtr projAB = std::make_shared<ProjectionJointSpaceToSE2>(space, spaceSE2, &manipulator);
179
180 // ompl::multilevel::ProjectionPtr projBC = std::make_shared<ompl::multilevel::Projection_SE2_R2>(spaceSE2, spaceR2);
181
182 // std::static_pointer_cast<ompl::multilevel::FiberedProjection>(projBC)->makeFiberSpace();
183
184 //#########################################################################
185 //## Put it all together
186 //#########################################################################
187 std::vector<SpaceInformationPtr> siVec;
188 std::vector<ompl::multilevel::ProjectionPtr> projVec;
189
190 // siVec.push_back(siR2); // Base Space R2
191 // projVec.push_back(projBC); // Projection R2 to SE2
192 siVec.push_back(siSE2); // Base Space SE2
193 projVec.push_back(projAB); // Projection SE2 to X
194 siVec.push_back(si); // State Space X
195
196 auto planner = std::make_shared<ompl::multilevel::QRRT>(siVec, projVec);
197
198 //#########################################################################
199 //## Set start state
200 //#########################################################################
201 ompl::base::State *start = si->allocState();
202 double *start_angles = start->as<PlanarManipulatorStateSpace::StateType>()->values;
203
204 for (int i = 0; i < numLinks; ++i)
205 {
206 start_angles[i] = 1e-1*(pow(-1,i)) + i*1e-3;
207 // start_angles[i] = 1e-7;//1e-1*(pow(-1,i)) + i*1e-3;
208 }
209
210 //#########################################################################
211 //## Set goal state
212 //#########################################################################
213 // 0.346324 0.0828153 2.96842 -2.17559 -0.718962 0.16532 -0.228314 0.172762 0.0471638 0.341137
214 ompl::base::State *goal = si->allocState();
215
216 std::vector<double> goalJoints;
217 manipulator.IK(goalJoints, goalFrame);
218
219 double *goal_angles = goal->as<PlanarManipulatorStateSpace::StateType>()->values;
220 goal_angles[0] = 0.346324;
221 goal_angles[1] = 0.0828153;
222 goal_angles[2] = 2.96842;
223 goal_angles[3] = -2.17559;
224 goal_angles[4] = -0.718962;
225 goal_angles[5] = 0.16532;
226 goal_angles[6] = -0.228314;
227 goal_angles[7] = 0.172762;
228
229 ProblemDefinitionPtr pdef = std::make_shared<ProblemDefinition>(si);
230 pdef->addStartState(start);
231 pdef->setGoalState(goal, 1e-3);
232
233 si->freeState(start);
234 si->freeState(goal);
235
236 //#########################################################################
237 //## Invoke planner
238 //#########################################################################
239 planner->setProblemDefinition(pdef);
240 planner->setup();
241
242 PlannerStatus status = planner->Planner::solve(timeout);
243
246 {
247 PathPtr path = pdef->getSolutionPath();
248 PathGeometric &pgeo = *static_cast<PathGeometric *>(path.get());
249 OMPL_INFORM("Solution path has %d states", pgeo.getStateCount());
250
251 pgeo.interpolate(250);
252 WriteVisualization(manipulator, &world, pgeo);
253 }
254}
A shared pointer wrapper for ompl::base::Path.
A shared pointer wrapper for ompl::base::ProblemDefinition.
The lower and upper bounds for an Rn space.
A state space representing SE(2)
A shared pointer wrapper for ompl::base::SpaceInformation.
A shared pointer wrapper for ompl::base::StateSpace.
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
Definition of a geometric path.
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
This namespace contains code that is specific to planning under geometric constraints.
This namespace contains datastructures and planners to exploit multilevel abstractions,...
@ PROJECTION_TASK_SPACE
X \rightarrow T (A mapping from state space X to a task space T)
Main namespace. Contains everything in this library.
A class to store the exit status of Planner::solve()
@ EXACT_SOLUTION
The planner found an exact solution.
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.