29 #ifndef HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH 30 #define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH 33 #include <hpp/util/debug.hh> 37 namespace pathOptimization {
41 :
J(outputSize, inputSize),
b(outputSize),
xSol(inputSize) {
49 assert(oc.
J.cols() ==
J.cols());
50 J.conservativeResize(
J.rows() + oc.
J.rows(),
J.cols());
51 J.bottomRows(oc.
J.rows()) = oc.
J;
52 b.conservativeResize(
b.rows() + oc.
b.rows());
53 b.tail(oc.
b.rows()) = oc.
b;
61 bool decompose(
bool check =
false,
bool throwIfNotValid =
false);
68 Eigen::FullPivLU<matrix_t> lu(
J);
82 lcr.
J.noalias() = lc.
J *
PK;
83 lcr.
b.noalias() = lc.
b - lc.
J *
xStar;
88 return lcr.
rank == std::min(lcr.
J.rows(), lcr.
J.cols());
107 Eigen::NumTraits<value_type>::dummy_precision()) {
109 if (err.isZero(threshold))
return true;
110 hppDout(error,
"constraints could not be satisfied: " << err.norm() <<
'\n' 117 J.conservativeResize(
J.rows() + nbRows,
J.cols());
118 b.conservativeResize(
b.rows() + nbRows);
120 J.bottomRows(nbRows).setZero();
149 #endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH void concatenate(const LinearConstraint &oc)
Definition: linear-constraint.hh:48
bool decompose(bool check=false, bool throwIfNotValid=false)
Definition: bi-rrt-planner.hh:35
pinocchio::size_type size_type
Definition: fwd.hh:162
matrix_t J
Definition: linear-constraint.hh:126
vector_t xSol
Definition: linear-constraint.hh:141
LinearConstraint(size_type inputSize, size_type outputSize)
Definition: linear-constraint.hh:40
void addRows(const std::size_t &nbRows)
Definition: linear-constraint.hh:115
bool isSatisfied(const vector_t &x, const value_type &threshold=Eigen::NumTraits< value_type >::dummy_precision())
Returns .
Definition: linear-constraint.hh:105
vector_t b
Definition: linear-constraint.hh:127
pinocchio::matrix_t matrix_t
Definition: fwd.hh:151
pinocchio::vector_t vector_t
Definition: fwd.hh:209
pinocchio::value_type value_type
Definition: fwd.hh:163
bool reduceConstraint(const LinearConstraint &lc, LinearConstraint &lcr, bool computeRank=true) const
Definition: linear-constraint.hh:80
void computeRank()
Compute rank of the constraint using a LU decomposition.
Definition: linear-constraint.hh:64
size_type rank
Rank of J.
Definition: linear-constraint.hh:136
void computeSolution(const vector_t &v)
Definition: linear-constraint.hh:97
A linear constraint .
Definition: linear-constraint.hh:39
vector_t xStar
is a particular solution.
Definition: linear-constraint.hh:141
matrix_t PK
Projector onto .
Definition: linear-constraint.hh:139