37 #include "ConstrainedPlanningCommon.h"
42 virtual void getStart(Eigen::VectorXd &x) = 0;
43 virtual void getGoal(Eigen::VectorXd &x) = 0;
49 ParallelChain(
const unsigned int n, Eigen::Vector3d offset,
unsigned int links,
unsigned int chainNum,
50 double length = 1,
double jointRadius = 0.2)
56 , jointRadius_(jointRadius)
62 void getStart(Eigen::VectorXd &x)
override
64 const double angle = boost::math::constants::pi<double>() / 16;
65 const unsigned int offset = 3 * links_ * chainNum_;
66 const Eigen::VectorXd axis =
67 Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_;
69 const Eigen::VectorXd step = Eigen::Vector3d::UnitZ() * length_;
70 Eigen::VectorXd joint = offset_ + Eigen::AngleAxisd(angle, axis) * step;
73 for (; i < links_; ++i)
75 x.segment(3 * i + offset, 3) = joint;
79 joint += Eigen::AngleAxisd(-angle, axis) * step;
83 void getGoal(Eigen::VectorXd &x)
override
85 unsigned int offset = 3 * links_ * chainNum_;
89 Eigen::VectorXd nstep = offset_ * length_;
90 Eigen::VectorXd estep =
91 Eigen::AngleAxisd(boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
93 Eigen::VectorXd sstep =
94 Eigen::AngleAxisd(boost::math::constants::pi<double>(), Eigen::Vector3d::UnitZ()) * offset_ * length_;
95 Eigen::VectorXd wstep =
96 Eigen::AngleAxisd(3 * boost::math::constants::pi<double>() / 2, Eigen::Vector3d::UnitZ()) * offset_ *
99 Eigen::VectorXd joint = offset_ + nstep;
100 x.segment(3 * 0 + offset, 3) = joint;
101 x.segment(3 * 1 + offset, 3) = x.segment(3 * 0 + offset, 3) + estep;
102 x.segment(3 * 2 + offset, 3) = x.segment(3 * 1 + offset, 3) + estep;
103 x.segment(3 * 3 + offset, 3) = x.segment(3 * 2 + offset, 3) + Eigen::Vector3d::UnitZ() * length_;
104 x.segment(3 * 4 + offset, 3) = x.segment(3 * 3 + offset, 3) + sstep;
105 x.segment(3 * 5 + offset, 3) = x.segment(3 * 4 + offset, 3) + sstep;
106 x.segment(3 * 6 + offset, 3) = x.segment(3 * 5 + offset, 3) + wstep;
110 Eigen::VectorXd step = offset_ * length_;
111 Eigen::VectorXd joint = offset_ + step;
114 for (; i < links_ / 2; ++i, joint += step)
115 x.segment(3 * i + offset, 3) = joint;
117 joint += Eigen::Vector3d::UnitZ() * length_ - step;
118 for (; i < links_; ++i, joint -= step)
119 x.segment(3 * i + offset, 3) = joint;
123 Eigen::Ref<const Eigen::VectorXd> getLink(
const Eigen::VectorXd &x,
const unsigned int idx)
const
125 const unsigned int offset = 3 * links_ * chainNum_;
126 return x.segment(offset + 3 * idx, 3);
129 void function(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out)
const override
131 unsigned int idx = 0;
133 Eigen::VectorXd j1 = offset_;
134 for (
unsigned int i = 0; i < links_; ++i)
136 const Eigen::VectorXd j2 = getLink(x, i);
137 out[idx++] = (j1 - j2).norm() - length_;
142 void jacobian(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out)
const override
144 const unsigned int offset = 3 * links_ * chainNum_;
147 Eigen::VectorXd plus(3 * (links_ + 1));
148 plus.head(3 * links_) = x.segment(offset, 3 * links_);
149 plus.tail(3) = Eigen::VectorXd::Zero(3);
151 Eigen::VectorXd minus(3 * (links_ + 1));
152 minus.head(3) = offset_;
153 minus.tail(3 * links_) = x.segment(offset, 3 * links_);
155 const Eigen::VectorXd diagonal = plus - minus;
157 for (
unsigned int i = 0; i < links_; i++)
158 out.row(i).segment(3 * i + offset, 3) = diagonal.segment(3 * i, 3).normalized();
160 out.block(1, offset, links_ - 1, 3 * links_ - 3) -= out.block(1, offset + 3, links_ - 1, 3 * links_ - 3);
164 const Eigen::Vector3d offset_;
165 const unsigned int links_;
166 const unsigned int chainNum_;
167 const double length_;
168 const double jointRadius_;
171 class ParallelPlatform :
public ob::Constraint,
public ParallelBase
174 ParallelPlatform(
unsigned int links,
unsigned int chains,
double radius = 1)
175 :
ob::
Constraint(3 * links * chains, chains), links_(links), chains_(chains), radius_(radius)
184 Eigen::Ref<const Eigen::VectorXd> getTip(
const Eigen::VectorXd &x,
unsigned int id)
const
186 return x.segment(3 * links_ * ((
id % chains_) + 1) - 3, 3);
189 void function(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out)
const override
193 out[0] = (getTip(x, 0) - getTip(x, 1)).norm() - radius_ * 2;
197 unsigned int idx = 0;
199 Eigen::Vector3d centroid = Eigen::Vector3d::Zero();
200 for (
unsigned int i = 0; i < chains_; ++i)
201 centroid += getTip(x, i);
204 for (
unsigned int i = 0; i < chains_; ++i)
205 out[idx++] = (centroid - getTip(x, i)).norm() - radius_;
207 for (
unsigned int i = 0; i < chains_ - 3; ++i)
209 const Eigen::Vector3d ab = getTip(x, i + 1) - getTip(x, i);
210 const Eigen::Vector3d ac = getTip(x, i + 2) - getTip(x, i);
211 const Eigen::Vector3d ad = getTip(x, i + 3) - getTip(x, i);
213 out[idx++] = ad.dot(ab.cross(ac));
217 void getStart(Eigen::VectorXd &x)
override
221 void getGoal(Eigen::VectorXd &x)
override
226 const unsigned int links_;
227 const unsigned int chains_;
228 const double radius_;
234 ParallelConstraint(
unsigned int links,
unsigned int chains,
double radius = 1,
double length = 1,
235 double jointRadius = 0.2)
241 , jointRadius_(jointRadius)
243 Eigen::Vector3d offset = Eigen::Vector3d::UnitX();
244 for (
unsigned int i = 0; i < chains_; ++i)
246 addConstraint(
new ParallelChain(chains * links * 3, offset, links, i, length, jointRadius));
248 Eigen::AngleAxisd(2 * boost::math::constants::pi<double>() / (
double)chains, Eigen::Vector3d::UnitZ()) *
252 addConstraint(
new ParallelPlatform(links, chains, radius));
255 void getStart(Eigen::VectorXd &x)
override
257 x = Eigen::VectorXd(3 * links_ * chains_);
259 dynamic_cast<ParallelBase *
>(constraints_[i])->getStart(x);
262 void getGoal(Eigen::VectorXd &x)
override
264 x = Eigen::VectorXd(3 * links_ * chains_);
266 dynamic_cast<ParallelBase *
>(constraints_[i])->getGoal(x);
269 ob::StateSpacePtr createSpace()
const
271 auto rvss = std::make_shared<ob::RealVectorStateSpace>(3 * links_ * chains_);
274 for (
unsigned int c = 0; c < chains_; ++c)
276 const unsigned int o = 3 * c * links_;
277 for (
int i = 0; i < (int)links_; ++i)
279 bounds.setLow(o + 3 * i + 0, -i - 2);
280 bounds.setHigh(o + 3 * i + 0, i + 2);
282 bounds.setLow(o + 3 * i + 1, -i - 2);
283 bounds.setHigh(o + 3 * i + 1, i + 2);
285 bounds.setLow(o + 3 * i + 2, -i - 2);
286 bounds.setHigh(o + 3 * i + 2, i + 2);
290 rvss->setBounds(bounds);
298 for (
unsigned int i = 0; i < links_ * chains_; ++i)
300 if (x.segment(3 * i, 3)[2] < 0)
304 for (
unsigned int i = 0; i < links_ * chains_ - 1; ++i)
306 if (x.segment(3 * i, 3).cwiseAbs().maxCoeff() < jointRadius_)
309 for (
unsigned int j = i + 1; j < links_ * chains_; ++j)
310 if ((x.segment(3 * i, 3) - x.segment(3 * j, 3)).cwiseAbs().maxCoeff() < jointRadius_)
319 ob::ProjectionEvaluatorPtr getProjection(ob::StateSpacePtr space)
const
324 ParallelProjection(ob::StateSpacePtr space,
unsigned int links,
unsigned int chains)
325 :
ob::ProjectionEvaluator(space), chains_(chains), links_(links)
329 unsigned int getDimension(
void)
const
334 void defaultCellSizes(
void)
336 cellSizes_.resize(1);
340 void project(
const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection)
const
344 for (
unsigned int i = 0; i < chains_; ++i)
345 projection(0) = x[3 * (i + 1) * links_ - 1];
347 projection(0) /= chains_;
351 const unsigned int chains_;
352 const unsigned int links_;
355 return std::make_shared<ParallelProjection>(space, links_, chains_);
358 void dump(std::ofstream &file)
const
360 file << links_ << std::endl;
361 file << chains_ << std::endl;
362 file << jointRadius_ << std::endl;
363 file << length_ << std::endl;
364 file << radius_ << std::endl;
374 const unsigned int links_;
375 const unsigned int chains_;
376 const double radius_;
377 const double length_;
378 const double jointRadius_;
381 bool parallelPlanningOnce(
ConstrainedProblem &cp,
enum PLANNER_TYPE planner,
bool output)
383 cp.setPlanner(planner,
"parallel");
390 OMPL_INFORM(
"Dumping problem information to `parallel_info.txt`.");
391 std::ofstream infofile(
"parallel_info.txt");
392 infofile << cp.type << std::endl;
393 dynamic_cast<ParallelConstraint *
>(cp.constraint.get())->dump(infofile);
402 bool parallelPlanningBench(
ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
404 cp.setupBenchmark(planners,
"parallel");
406 auto parallel =
dynamic_cast<ParallelConstraint *
>(cp.constraint.get());
407 parallel->addBenchmarkParameters(cp.bench);
414 bool parallelPlanning(
bool output,
enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
unsigned int links,
418 auto constraint = std::make_shared<ParallelConstraint>(links, chains);
421 cp.setConstrainedOptions(c_opt);
422 cp.setAtlasOptions(a_opt);
424 cp.css->registerProjection(
"parallel", constraint->getProjection(cp.css));
426 Eigen::VectorXd start, goal;
427 constraint->getStart(start);
428 constraint->getGoal(goal);
430 cp.setStartAndGoalStates(start, goal);
431 cp.ss->setStateValidityChecker(std::bind(&ParallelConstraint::isValid, constraint, std::placeholders::_1));
434 return parallelPlanningOnce(cp, planners[0], output);
436 return parallelPlanningBench(cp, planners);
439 auto help_msg =
"Shows this help message.";
440 auto output_msg =
"Dump found solution path (if one exists) in plain text to `parallel_path.txt`. "
441 "Problem information is dumped to `parallel_info`.txt";
442 auto links_msg =
"Number of links in each kinematic chain. Minimum is 3. Must be odd.";
443 auto chains_msg =
"Number of chains in parallel mechanism. Minimum is 2.";
444 auto bench_msg =
"Do benchmarking on provided planner list.";
446 int main(
int argc,
char **argv)
449 enum SPACE_TYPE space = PJ;
450 std::vector<enum PLANNER_TYPE> planners = {RRT};
452 unsigned int links = 3;
453 unsigned int chains = 4;
458 po::options_description desc(
"Options");
459 desc.add_options()(
"help,h", help_msg);
460 desc.add_options()(
"output,o", po::bool_switch(&output)->default_value(
false), output_msg);
461 desc.add_options()(
"links,l", po::value<unsigned int>(&links)->default_value(3), links_msg);
462 desc.add_options()(
"chains,c", po::value<unsigned int>(&chains)->default_value(4), chains_msg);
463 desc.add_options()(
"bench", po::bool_switch(&bench)->default_value(
false), bench_msg);
465 addSpaceOption(desc, &space);
466 addPlannerOption(desc, &planners);
467 addConstrainedOptions(desc, &c_opt);
468 addAtlasOptions(desc, &a_opt);
470 po::variables_map vm;
471 po::store(po::parse_command_line(argc, argv, desc), vm);
474 if (vm.count(
"help"))
476 std::cout << desc << std::endl;
480 parallelPlanning(output, space, planners, links, chains, c_opt, a_opt, bench);