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 
63 using namespace ompl::geometric;
64 
65 namespace ompl
66 {
67  namespace multilevel
68  {
69  OMPL_CLASS_FORWARD(Projection);
70  }
71 }
72 
73 class ProjectionJointSpaceToSE2 : public ompl::multilevel::Projection
74 {
75 public:
76  ProjectionJointSpaceToSE2(StateSpacePtr bundle, StateSpacePtr base, PlanarManipulator *manip)
77  : Projection(bundle, base), manip_(manip)
78  {
79  type_ = ompl::multilevel::PROJECTION_TASK_SPACE;
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 
122 private:
123  PlanarManipulator *manip_;
124 };
125 
126 int 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 
244  if (status == ompl::base::PlannerStatus::EXACT_SOLUTION ||
245  status == ompl::base::PlannerStatus::APPROXIMATE_SOLUTION)
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 }
PathPtr
A shared pointer wrapper for ompl::base::Path.
ompl::base::PlannerStatus::APPROXIMATE_SOLUTION
@ APPROXIMATE_SOLUTION
The planner found an approximate solution.
Definition: PlannerStatus.h:192
ompl::multilevel::PROJECTION_TASK_SPACE
@ PROJECTION_TASK_SPACE
X \rightarrow T (A mapping from state space X to a task space T)
Definition: ProjectionTypes.h:209
ompl::geometric
This namespace contains code that is specific to planning under geometric constraints.
Definition: GeneticSearch.h:79
SpaceInformationPtr
A shared pointer wrapper for ompl::control::SpaceInformation.
ompl::base::State
Definition of an abstract state.
Definition: State.h:113
OMPL_INFORM
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition: Console.h:68
ompl::base::State::as
const T * as() const
Cast this instance to a desired type.
Definition: State.h:162
ompl::geometric::PathGeometric
Definition of a geometric path.
Definition: PathGeometric.h:97
ompl::base::PlannerStatus
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:112
ompl::geometric::PathGeometric::getStateCount
std::size_t getStateCount() const
Get the number of states (way-points) that make up this path.
Definition: PathGeometric.h:292
ompl::base::SE2StateSpace
A state space representing SE(2)
Definition: SE2StateSpace.h:113
ProblemDefinitionPtr
A shared pointer wrapper for ompl::base::ProblemDefinition.
ompl::base::PlannerStatus::EXACT_SOLUTION
@ EXACT_SOLUTION
The planner found an exact solution.
Definition: PlannerStatus.h:194
ompl::base::SE2StateSpace::StateType
A state in SE(2): (x, y, yaw)
Definition: SE2StateSpace.h:149
StateSpacePtr
A shared pointer wrapper for ompl::base::StateSpace.
ompl::geometric::PathGeometric::interpolate
void interpolate(unsigned int count)
Insert a number of states in a path so that the path is made up of exactly count states....
Definition: PathGeometric.cpp:337
ompl
Main namespace. Contains everything in this library.
Definition: AppBase.h:21
ompl::base::RealVectorBounds
The lower and upper bounds for an Rn space.
Definition: RealVectorBounds.h:111