Loading...
Searching...
No Matches
MultiLevelPlanarManipulatorCommon.h
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
52#include <ompl/geometric/PathGeometric.h>
53#include <ompl/base/spaces/SE2StateSpace.h>
54#include <ompl/base/spaces/RealVectorStateSpace.h>
55#include <ompl/multilevel/datastructures/Projection.h>
56#include <ompl/multilevel/datastructures/projections/SE2_R2.h>
57
58using namespace ompl::base;
59using namespace ompl::geometric;
60
61const int numLinks = 9;
62const double timeout = 10;
63const int xySlices = std::max(2, numLinks / 3);
64const std::string problemName = "corridor";
65const bool viz = false;
66
67void WriteVisualization(const PlanarManipulator &manipulator, const PolyWorld *world, const PathGeometric &path)
68{
69 const int numLinks = manipulator.getNumLinks();
70
71 const char *world_file = "world.yaml";
72 OMPL_INFORM("Writing world to %s", world_file);
73 world->writeWorld(world_file);
74
75 const double linkLength = 1.0 / numLinks;
76 const Eigen::Affine2d &basePose = manipulator.getBaseFrame();
77
78 const char *path_file = "manipulator_path.txt";
79 OMPL_INFORM("Writing path to %s", path_file);
80 std::ofstream fout;
81
82 fout.open(path_file);
83 fout << numLinks << " " << linkLength << " " << basePose.translation()(0) << " " << basePose.translation()(1) << " "
84 << xySlices << std::endl;
85
86 // Write each state on the interpolated path.
87 for (size_t i = 0; i < path.getStateCount(); ++i)
88 {
89 const double *angles = path.getState(i)->as<PlanarManipulatorStateSpace::StateType>()->values;
90 for (size_t j = 0; j < manipulator.getNumLinks(); ++j)
91 fout << angles[j] << " ";
92 fout << std::endl;
93 }
94 fout.close();
95}
96
97class R2CollisionChecker : public ompl::base::StateValidityChecker
98{
99public:
100 R2CollisionChecker(const ompl::base::SpaceInformationPtr &si, const PolyWorld *world)
101 : ompl::base::StateValidityChecker(si), world_(world)
102 {
103 }
104
105 ~R2CollisionChecker() = default;
106
107 virtual bool isValid(const ompl::base::State *state) const
108 {
109 const double *angles = state->as<RealVectorStateSpace::StateType>()->values;
110 std::vector<Point> coordinates;
111 coordinates.push_back({angles[0], angles[1]});
112
113 //(1) check out of bounds
114 if (world_->outOfBounds(coordinates[0]))
115 {
116 return false;
117 }
118
119 // Check each coordinate for obstacle intersection.
120 for (size_t j = 0; j < world_->numObstacles(); ++j)
121 if (world_->obstacle(j).inside(coordinates[0]))
122 return false;
123
124 return true;
125 }
126
127private:
128 const PolyWorld *world_;
129};
130class SE2CollisionChecker : public ompl::base::StateValidityChecker
131{
132public:
133 SE2CollisionChecker(const ompl::base::SpaceInformationPtr &si, const PolyWorld *world)
134 : ompl::base::StateValidityChecker(si), world_(world)
135 {
136 }
137
138 ~SE2CollisionChecker() = default;
139
140 virtual bool isValid(const ompl::base::State *state) const
141 {
142 const double x = state->as<SE2StateSpace::StateType>()->getX();
143 const double y = state->as<SE2StateSpace::StateType>()->getY();
144
145 std::vector<Point> coordinates;
146 coordinates.push_back({x, y});
147
148 //(1) check out of bounds
149 if (world_->outOfBounds(coordinates[0]))
150 return false;
151
152 // Check each coordinate for obstacle intersection.
153 for (size_t j = 0; j < world_->numObstacles(); ++j)
154 if (world_->obstacle(j).inside(coordinates[0]))
155 return false;
156
157 return true;
158 }
159
160private:
161 const PolyWorld *world_;
162};
virtual bool isValid(const ompl::base::State *state) const
Return true if the state state is valid. Usually, this means at least collision checking....
virtual bool isValid(const ompl::base::State *state) const
Return true if the state state is valid. Usually, this means at least collision checking....
A state in SE(2): (x, y, yaw)
Abstract definition for a class checking the validity of states. The implementation of this class mus...
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.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
Definition Console.h:68
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.