Loading...
Searching...
No Matches
ConstrainedPlanningKinematicChain.cpp
1/*********************************************************************
2* Software License Agreement (BSD License)
3*
4* Copyright (c) 2018, Rice University
5* All rights reserved.
6*
7* Redistribution and use in source and binary forms, with or without
8* modification, are permitted provided that the following conditions
9* are met:
10*
11* * Redistributions of source code must retain the above copyright
12* notice, this list of conditions and the following disclaimer.
13* * Redistributions in binary form must reproduce the above
14* copyright notice, this list of conditions and the following
15* disclaimer in the documentation and/or other materials provided
16* with the distribution.
17* * Neither the name of the Rice University nor the names of its
18* contributors may be used to endorse or promote products derived
19* from this software without specific prior written permission.
20*
21* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32* POSSIBILITY OF SUCH DAMAGE.
33*********************************************************************/
34
35/* Author: Zachary Kingston */
36
37#include "../KinematicChain.h"
38#include "ConstrainedPlanningCommon.h"
39
40class ConstrainedKinematicChainValidityChecker : public KinematicChainValidityChecker
41{
42public:
43 ConstrainedKinematicChainValidityChecker(const ob::ConstrainedSpaceInformationPtr &si)
45 {
46 }
47
48 bool isValid(const ob::State *state) const override
49 {
50 auto &&space = si_->getStateSpace()->as<ob::ConstrainedStateSpace>()->getSpace()->as<KinematicChainSpace>();
51 auto &&s = state->as<ob::ConstrainedStateSpace::StateType>()->getState()->as<KinematicChainSpace::StateType>();
52 return isValidImpl(space, s);
53 }
54};
55
56class KinematicChainConstraint : public ob::Constraint
57{
58public:
59 KinematicChainConstraint(unsigned int links, double linkLength) : ob::Constraint(links, 1), linkLength_(linkLength)
60 {
61 }
62
63 void function(const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out) const override
64 {
65 Eigen::Vector2d e = Eigen::Vector2d::Zero(), eN = Eigen::Vector2d::Zero();
66 double theta = 0.;
67
68 for (unsigned int i = 0; i < x.size(); ++i)
69 {
70 theta += x[i];
71 eN[0] = e[0] + cos(theta) * linkLength_;
72 eN[1] = e[1] + sin(theta) * linkLength_;
73 e = eN;
74 }
75
76 out[0] = e[1] - linkLength_;
77 }
78
79private:
80 double linkLength_;
81};
82
83bool chainPlanningOnce(ConstrainedProblem &cp, enum PLANNER_TYPE planner, bool output)
84{
85 cp.setPlanner(planner);
86
87 // Solve the problem
88 ob::PlannerStatus stat = cp.solveOnce(true);
89
90 if (output && stat)
91 {
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();
95 path.interpolate();
96 std::ofstream pathfile(filename);
97 path.printAsMatrix(pathfile);
98 pathfile.close();
99 }
100
101 cp.atlasStats();
102
103 return stat;
104}
105
106bool chainPlanningBench(ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
107{
108 cp.setupBenchmark(planners, "kinematic");
109 cp.bench->addExperimentParameter("links", "INTEGER", std::to_string(cp.constraint->getAmbientDimension()));
110
111 cp.runBenchmark();
112 return false;
113}
114
115bool chainPlanning(bool output, enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners, unsigned int links,
116 struct ConstrainedOptions &c_opt, struct AtlasOptions &a_opt, bool bench)
117{
118 Environment env = createEmptyEnvironment(links);
119
120 // Create the ambient space state space for the problem.
121 auto ss = std::make_shared<KinematicChainSpace>(links, 1. / (double)links, &env);
122
123 // Create a shared pointer to our constraint.
124 auto constraint = std::make_shared<KinematicChainConstraint>(links, 1. / (double)links);
125
126 ConstrainedProblem cp(space, ss, constraint);
127 cp.setConstrainedOptions(c_opt);
128 cp.setAtlasOptions(a_opt);
129
130 Eigen::VectorXd start, goal;
131 start = Eigen::VectorXd::Constant(links, 0);
132 goal = Eigen::VectorXd::Constant(links, 0);
133
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;
137
138 cp.setStartAndGoalStates(start, goal);
139 cp.ss->setStateValidityChecker(std::make_shared<ConstrainedKinematicChainValidityChecker>(cp.csi));
140
141 if (!bench)
142 return chainPlanningOnce(cp, planners[0], output);
143 else
144 return chainPlanningBench(cp, planners);
145}
146
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.";
152
153int main(int argc, char **argv)
154{
155 bool output, bench;
156 enum SPACE_TYPE space = PJ;
157 std::vector<enum PLANNER_TYPE> planners = {RRT};
158
159 struct ConstrainedOptions c_opt;
160 struct AtlasOptions a_opt;
161
162 unsigned int links;
163
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);
169
170 addSpaceOption(desc, &space);
171 addPlannerOption(desc, &planners);
172 addConstrainedOptions(desc, &c_opt);
173 addAtlasOptions(desc, &a_opt);
174
175 po::variables_map vm;
176 po::store(po::parse_command_line(argc, argv, desc), vm);
177 po::notify(vm);
178
179 if (vm.count("help") != 0u)
180 {
181 std::cout << desc << std::endl;
182 return 1;
183 }
184
185 return static_cast<int>(chainPlanning(output, space, planners, links, c_opt, a_opt, bench));
186}
bool isValid(const ompl::base::State *state) const override
Return true if the state state is valid. Usually, this means at least collision checking....
A StateSpace that has a Constraint imposed upon it. Underlying space functions are passed to the ambi...
Definition of a differentiable holonomic constraint on a configuration space. See Constrained Plannin...
Definition: Constraint.h:76
const StateSpacePtr & getStateSpace() const
Return the instance of the used state space.
T * as()
Cast this instance to a desired type.
Definition: StateSpace.h:87
SpaceInformation * si_
The instance of space information this state validity checker operates on.
Definition of an abstract state.
Definition: State.h:50
const T * as() const
Cast this instance to a desired type.
Definition: State.h:66
void addExperimentParameter(const std::string &name, const std::string &type, const std::string &value)
Add an optional parameter's information to the benchmark output. Useful for aggregating results over ...
Definition: Benchmark.h:218
#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...
A class to store the exit status of Planner::solve()
Definition: PlannerStatus.h:49