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"
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>
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;
69 const int numLinks = manipulator.getNumLinks();
71 const char *world_file =
"world.yaml";
73 world->writeWorld(world_file);
75 const double linkLength = 1.0 / numLinks;
76 const Eigen::Affine2d &basePose = manipulator.getBaseFrame();
78 const char *path_file =
"manipulator_path.txt";
83 fout << numLinks <<
" " << linkLength <<
" " << basePose.translation()(0) <<
" " << basePose.translation()(1) <<
" "
84 << xySlices << std::endl;
87 for (
size_t i = 0; i < path.getStateCount(); ++i)
90 for (
size_t j = 0; j < manipulator.getNumLinks(); ++j)
91 fout << angles[j] <<
" ";
110 std::vector<Point> coordinates;
111 coordinates.push_back({angles[0], angles[1]});
114 if (world_->outOfBounds(coordinates[0]))
120 for (
size_t j = 0; j < world_->numObstacles(); ++j)
121 if (world_->obstacle(j).inside(coordinates[0]))
145 std::vector<Point> coordinates;
146 coordinates.push_back({x, y});
149 if (world_->outOfBounds(coordinates[0]))
153 for (
size_t j = 0; j < world_->numObstacles(); ++j)
154 if (world_->obstacle(j).inside(coordinates[0]))
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....
The definition of a state in Rn
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.
const T * as() const
Cast this instance to a desired type.
Definition of a geometric path.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
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.