37#include "../KinematicChain.h"
38#include "ConstrainedPlanningCommon.h"
43 ConstrainedKinematicChainValidityChecker(
const ob::ConstrainedSpaceInformationPtr &si)
44 : KinematicChainValidityChecker(si)
48 bool isValid(
const ob::State *state)
const override
50 auto &&space =
si_->getStateSpace()->as<ob::ConstrainedStateSpace>()->getSpace()->as<KinematicChainSpace>();
52 return isValidImpl(space, s);
59 KinematicChainConstraint(
unsigned int links,
double linkLength) : ob::Constraint(links, 1), linkLength_(linkLength)
63 void function(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out)
const override
65 Eigen::Vector2d e = Eigen::Vector2d::Zero(), eN = Eigen::Vector2d::Zero();
68 for (
unsigned int i = 0; i < x.size(); ++i)
71 eN[0] = e[0] + cos(theta) * linkLength_;
72 eN[1] = e[1] + sin(theta) * linkLength_;
76 out[0] = e[1] - linkLength_;
85 cp.setPlanner(planner);
92 auto filename = boost::str(boost::format(
"kinematic_path_%i.dat") % cp.constraint->getAmbientDimension());
93 OMPL_INFORM(
"Dumping problem information to `%s`.", filename.c_str());
94 auto path = cp.ss->getSolutionPath();
96 std::ofstream pathfile(filename);
97 path.printAsMatrix(pathfile);
106bool chainPlanningBench(
ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
108 cp.setupBenchmark(planners,
"kinematic");
109 cp.bench->
addExperimentParameter(
"links",
"INTEGER", std::to_string(cp.constraint->getAmbientDimension()));
115bool chainPlanning(
bool output,
enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
unsigned int links,
118 Environment env = createEmptyEnvironment(links);
121 auto ss = std::make_shared<KinematicChainSpace>(links, 1. / (
double)links, &env);
124 auto constraint = std::make_shared<KinematicChainConstraint>(links, 1. / (
double)links);
127 cp.setConstrainedOptions(c_opt);
128 cp.setAtlasOptions(a_opt);
130 Eigen::VectorXd start, goal;
131 start = Eigen::VectorXd::Constant(links, 0);
132 goal = Eigen::VectorXd::Constant(links, 0);
134 start[links - 1] = boost::math::constants::pi<double>() / 2;
135 goal[0] = boost::math::constants::pi<double>();
136 goal[links - 1] = -boost::math::constants::pi<double>() / 2;
138 cp.setStartAndGoalStates(start, goal);
139 cp.ss->setStateValidityChecker(std::make_shared<ConstrainedKinematicChainValidityChecker>(cp.csi));
142 return chainPlanningOnce(cp, planners[0], output);
144 return chainPlanningBench(cp, planners);
147auto help_msg =
"Shows this help message.";
148auto output_msg =
"Dump found solution path (if one exists) and environment to data files that can be rendered with "
149 "`KinematicChainPathPlot.py`.";
150auto links_msg =
"Number of links in kinematic chain.";
151auto bench_msg =
"Do benchmarking on provided planner list.";
153int main(
int argc,
char **argv)
156 enum SPACE_TYPE space = PJ;
157 std::vector<enum PLANNER_TYPE> planners = {RRT};
164 po::options_description desc(
"Options");
165 desc.add_options()(
"help,h", help_msg);
166 desc.add_options()(
"output,o", po::bool_switch(&output)->default_value(
false), output_msg);
167 desc.add_options()(
"links,l", po::value<unsigned int>(&links)->default_value(5), links_msg);
168 desc.add_options()(
"bench", po::bool_switch(&bench)->default_value(
false), bench_msg);
170 addSpaceOption(desc, &space);
171 addPlannerOption(desc, &planners);
172 addConstrainedOptions(desc, &c_opt);
173 addAtlasOptions(desc, &a_opt);
175 po::variables_map vm;
176 po::store(po::parse_command_line(argc, argv, desc), vm);
179 if (vm.count(
"help") != 0u)
181 std::cout << desc << std::endl;
185 return static_cast<int>(chainPlanning(output, space, planners, links, c_opt, a_opt, bench));
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
ompl::base::State StateType
Define the type of state allocated by this space.
SpaceInformation * si_
The instance of space information this state validity checker operates on.
const T * as() const
Cast this instance to a desired type.
#define OMPL_INFORM(fmt,...)
Log a formatted information string.
A class to store the exit status of Planner::solve()