37 #include "ConstrainedPlanningCommon.h"
48 Wall(
double offset,
double thickness,
double width,
double joint_radius,
unsigned int type)
49 : offset_(offset), thickness_(thickness + joint_radius), width_(width + joint_radius), type_(type)
56 bool within(
double x)
const
58 if (x < (offset_ - thickness_) || x > (offset_ + thickness_))
63 bool checkJoint(
const Eigen::Ref<const Eigen::VectorXd> &v)
const
65 double x = v[0], y = v[1], z = v[2];
91 const double thickness_;
93 const unsigned int type_;
96 const double WALL_WIDTH = 0.5;
97 const double JOINT_RADIUS = 0.2;
98 const double LINK_LENGTH = 1.0;
113 ChainConstraint(
unsigned int links,
unsigned int obstacles = 0,
unsigned int extra = 1)
116 , length_(LINK_LENGTH)
119 , jointRadius_(JOINT_RADIUS)
120 , obstacles_(obstacles)
123 double step = 2 * radius_ / (double)(obstacles_ + 1);
124 double current = -radius_ + step;
126 for (
unsigned int i = 0; i < obstacles_; i++, current += step)
127 walls_.emplace_back(current, radius_ / 8, WALL_WIDTH, JOINT_RADIUS, i % 2);
130 void function(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::VectorXd> out)
const override
132 Eigen::VectorXd joint1 = Eigen::VectorXd::Zero(3);
133 for (
unsigned int i = 0; i < links_; i++)
135 auto &&joint2 = x.segment(3 * i, 3);
136 out[i] = (joint1 - joint2).norm() - length_;
141 out[links_] = x.tail(3).norm() - radius_;
143 const unsigned int o = links_ - 5;
146 out[links_ + 1] = x[(o + 0) * 3 + 2] - x[(o + 1) * 3 + 2];
148 out[links_ + 2] = x[(o + 1) * 3 + 0] - x[(o + 2) * 3 + 0];
150 out[links_ + 3] = x[(o + 2) * 3 + 2] - x[(o + 3) * 3 + 2];
153 void jacobian(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out)
const override
157 Eigen::VectorXd plus(3 * (links_ + 1));
158 plus.head(3 * links_) = x.segment(0, 3 * links_);
159 plus.tail(3) = Eigen::VectorXd::Zero(3);
161 Eigen::VectorXd minus(3 * (links_ + 1));
162 minus.head(3) = Eigen::VectorXd::Zero(3);
163 minus.tail(3 * links_) = x.segment(0, 3 * links_);
165 auto &&diagonal = plus - minus;
167 for (
unsigned int i = 0; i < links_; i++)
168 out.row(i).segment(3 * i + 0, 3) = diagonal.segment(3 * i, 3).normalized();
170 out.block(1, 0, links_ - 1, 3 * links_ - 3) -= out.block(1, 3, links_ - 1, 3 * links_ - 3);
173 out.row(links_).tail(3) = -diagonal.tail(3).normalized().transpose();
175 const unsigned int o = links_ - 5;
179 out(links_ + 1, (o + 0) * 3 + 2) = 1;
180 out(links_ + 1, (o + 1) * 3 + 2) = -1;
184 out(links_ + 2, (o + 1) * 3 + 0) = 1;
185 out(links_ + 2, (o + 2) * 3 + 0) = -1;
189 out(links_ + 3, (o + 2) * 3 + 2) = 1;
190 out(links_ + 3, (o + 3) * 3 + 2) = -1;
201 for (
unsigned int i = 0; i < links_; i++)
203 auto &&joint = x.segment(3 * i, 3);
207 if (joint.norm() >= (radius_ - jointRadius_))
208 for (
auto wall : walls_)
209 if (!wall.checkJoint(joint))
213 for (
unsigned int i = 0; i < links_ - 1; i++)
215 auto &&joint1 = x.segment(3 * i, 3);
216 if (joint1.cwiseAbs().maxCoeff() < jointRadius_)
219 for (
unsigned int j = i + 1; j < links_; j++)
221 auto &&joint2 = x.segment(3 * j, 3);
222 if ((joint1 - joint2).cwiseAbs().maxCoeff() < jointRadius_)
230 ob::StateSpacePtr createSpace()
const
232 auto rvss = std::make_shared<ob::RealVectorStateSpace>(3 * links_);
235 for (
int i = 0; i < (int)links_; ++i)
237 bounds.setLow(3 * i + 0, -i - 1);
238 bounds.setHigh(3 * i + 0, i + 1);
240 bounds.setLow(3 * i + 1, -i - 1);
241 bounds.setHigh(3 * i + 1, i + 1);
243 bounds.setLow(3 * i + 2, -i - 1);
244 bounds.setHigh(3 * i + 2, i + 1);
247 rvss->setBounds(bounds);
251 void setStartAndGoalStates(Eigen::VectorXd &start, Eigen::VectorXd &goal)
const
253 start = Eigen::VectorXd(3 * links_);
254 goal = Eigen::VectorXd(3 * links_);
257 for (; i < (int)links_ - 3; ++i)
259 start[3 * i] = i + 1;
260 start[3 * i + 1] = 0;
261 start[3 * i + 2] = 0;
263 goal[3 * i] = -(i + 1);
269 start[3 * i + 1] = -1;
270 start[3 * i + 2] = 0;
279 start[3 * i + 1] = -1;
280 start[3 * i + 2] = 0;
288 start[3 * i] = i - 1;
289 start[3 * i + 1] = 0;
290 start[3 * i + 2] = 0;
292 goal[3 * i] = -(i - 1);
300 ob::ProjectionEvaluatorPtr getProjection(ob::StateSpacePtr space)
const
305 ChainProjection(ob::StateSpacePtr space,
unsigned int links,
double radius)
306 :
ob::ProjectionEvaluator(space), links_(links), radius_(radius)
310 unsigned int getDimension(
void)
const
315 void defaultCellSizes(
void)
317 cellSizes_.resize(2);
322 void project(
const ob::State *state, Eigen::Ref<Eigen::VectorXd> projection)
const
325 const unsigned int s = 3 * (links_ - 1);
327 projection(0) = atan2(x[s + 1], x[s]);
328 projection(1) = acos(x[s + 2] / radius_);
332 const unsigned int links_;
336 return std::make_shared<ChainProjection>(space, links_, radius_);
339 void dump(std::ofstream &file)
const
341 file << links_ << std::endl;
342 file << obstacles_ << std::endl;
343 file << extra_ << std::endl;
344 file << jointRadius_ << std::endl;
345 file << length_ << std::endl;
346 file << radius_ << std::endl;
347 file << width_ << std::endl;
358 const unsigned int links_;
359 const double length_;
361 const double radius_;
362 const double jointRadius_;
363 const unsigned int obstacles_;
364 const unsigned int extra_;
365 std::vector<Wall> walls_;
368 bool chainPlanningOnce(
ConstrainedProblem &cp,
enum PLANNER_TYPE planner,
bool output)
370 cp.setPlanner(planner,
"chain");
377 OMPL_INFORM(
"Dumping problem information to `chain_info.txt`.");
378 std::ofstream infofile(
"chain_info.txt");
379 infofile << cp.type << std::endl;
380 dynamic_cast<ChainConstraint *
>(cp.constraint.get())->dump(infofile);
389 bool chainPlanningBench(
ConstrainedProblem &cp, std::vector<enum PLANNER_TYPE> &planners)
391 cp.setupBenchmark(planners,
"chain");
393 auto chain =
dynamic_cast<ChainConstraint *
>(cp.constraint.get());
394 chain->addBenchmarkParameters(cp.bench);
401 bool chainPlanning(
bool output,
enum SPACE_TYPE space, std::vector<enum PLANNER_TYPE> &planners,
unsigned int links,
406 auto constraint = std::make_shared<ChainConstraint>(links, obstacles, extra);
409 cp.setConstrainedOptions(c_opt);
410 cp.setAtlasOptions(a_opt);
412 cp.css->registerProjection(
"chain", constraint->getProjection(cp.css));
414 Eigen::VectorXd start, goal;
415 constraint->setStartAndGoalStates(start, goal);
417 cp.setStartAndGoalStates(start, goal);
418 cp.ss->setStateValidityChecker(std::bind(&ChainConstraint::isValid, constraint, std::placeholders::_1));
421 return chainPlanningOnce(cp, planners[0], output);
423 return chainPlanningBench(cp, planners);
426 auto help_msg =
"Shows this help message.";
427 auto output_msg =
"Dump found solution path (if one exists) in plain text to `chain_path.txt`. "
428 "Problem information is dumped to `chain_info`.txt";
429 auto links_msg =
"Number of links in the kinematic chain. Minimum is 4.";
430 auto obstacles_msg =
"Number of `wall' obstacles on the surface of the sphere. Ranges from [0, 2]";
431 auto extra_msg =
"Number of extra constraints to add to the chain. Extra constraints are as follows:\n"
432 "1: End-effector is constrained to be on the surface of a sphere of radius links - 2\n"
433 "2: (links-5)th and (links-4)th ball have the same z-value\n"
434 "3: (links-4)th and (links-3)th ball have the same x-value\n"
435 "4: (links-3)th and (links-2)th ball have the same z-value";
436 auto bench_msg =
"Do benchmarking on provided planner list.";
438 int main(
int argc,
char **argv)
441 enum SPACE_TYPE space = PJ;
442 std::vector<enum PLANNER_TYPE> planners = {RRT};
444 unsigned int links = 5;
445 unsigned int obstacles = 0;
446 unsigned int extra = 1;
451 po::options_description desc(
"Options");
452 desc.add_options()(
"help,h", help_msg);
453 desc.add_options()(
"output,o", po::bool_switch(&output)->default_value(
false), output_msg);
454 desc.add_options()(
"links,l", po::value<unsigned int>(&links)->default_value(5), links_msg);
455 desc.add_options()(
"obstacles,x", po::value<unsigned int>(&obstacles)->default_value(0), obstacles_msg);
456 desc.add_options()(
"extra,e", po::value<unsigned int>(&extra)->default_value(1), extra_msg);
457 desc.add_options()(
"bench", po::bool_switch(&bench)->default_value(
false), bench_msg);
459 addSpaceOption(desc, &space);
460 addPlannerOption(desc, &planners);
461 addConstrainedOptions(desc, &c_opt);
462 addAtlasOptions(desc, &a_opt);
464 po::variables_map vm;
465 po::store(po::parse_command_line(argc, argv, desc), vm);
468 if (vm.count(
"help"))
470 std::cout << desc << std::endl;
474 chainPlanning(output, space, planners, links, obstacles, extra, c_opt, a_opt, bench);