37#include "ompl/base/Constraint.h"
38#include "ompl/base/spaces/constraint/ConstrainedStateSpace.h"
40void ompl::base::Constraint::function(
const State *state, Eigen::Ref<Eigen::VectorXd> out)
const
42 function(*state->as<ConstrainedStateSpace::StateType>(), out);
45void ompl::base::Constraint::jacobian(
const State *state, Eigen::Ref<Eigen::MatrixXd> out)
const
47 jacobian(*state->as<ConstrainedStateSpace::StateType>(), out);
50void ompl::base::Constraint::jacobian(
const Eigen::Ref<const Eigen::VectorXd> &x, Eigen::Ref<Eigen::MatrixXd> out)
const
52 Eigen::VectorXd y1 = x;
53 Eigen::VectorXd y2 = x;
54 Eigen::VectorXd t1(getCoDimension());
55 Eigen::VectorXd t2(getCoDimension());
58 for (std::size_t j = 0; j < n_; j++)
60 const double ax = std::fabs(x[j]);
62 const double h = std::sqrt(std::numeric_limits<double>::epsilon()) * (ax >= 1 ? ax : 1);
69 const Eigen::VectorXd m1 = (t1 - t2) / (y1[j] - y2[j]);
74 const Eigen::VectorXd m2 = (t1 - t2) / (y1[j] - y2[j]);
79 const Eigen::VectorXd m3 = (t1 - t2) / (y1[j] - y2[j]);
81 out.col(j) = 1.5 * m1 - 0.6 * m2 + 0.1 * m3;
88bool ompl::base::Constraint::project(State *state)
const
90 return project(*state->as<ConstrainedStateSpace::StateType>());
93bool ompl::base::Constraint::project(Eigen::Ref<Eigen::VectorXd> x)
const
96 unsigned int iter = 0;
98 Eigen::VectorXd f(getCoDimension());
99 Eigen::MatrixXd j(getCoDimension(), n_);
101 const double squaredTolerance = tolerance_ * tolerance_;
104 while ((norm = f.squaredNorm()) > squaredTolerance && iter++ < maxIterations_)
107 x -= j.jacobiSvd(Eigen::ComputeThinU | Eigen::ComputeThinV).solve(f);
111 return norm < squaredTolerance;
114double ompl::base::Constraint::distance(
const State *state)
const
116 return distance(*state->as<ConstrainedStateSpace::StateType>());
119double ompl::base::Constraint::distance(
const Eigen::Ref<const Eigen::VectorXd> &x)
const
121 Eigen::VectorXd f(getCoDimension());
126bool ompl::base::Constraint::isSatisfied(
const State *state)
const
128 return isSatisfied(*state->as<ConstrainedStateSpace::StateType>());
131bool ompl::base::Constraint::isSatisfied(
const Eigen::Ref<const Eigen::VectorXd> &x)
const
133 Eigen::VectorXd f(getCoDimension());
135 return f.allFinite() && f.squaredNorm() <= tolerance_ * tolerance_;