hpp-core: include/hpp/core/path-optimization/linear-constraint.hh Source File

hpp-core 9.0.2 Implement basic classes for canonical path planning for kinematic chains.

Loading...
Searching...
No Matches
linear-constraint.hh
Go to the documentation of this file.
1// Copyright (c) 2017, Joseph Mirabel
2// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
3//
4
5// Redistribution and use in source and binary forms, with or without
6// modification, are permitted provided that the following conditions are
7// met:
8//
9// 1. Redistributions of source code must retain the above copyright
10// notice, this list of conditions and the following disclaimer.
11//
12// 2. Redistributions in binary form must reproduce the above copyright
13// notice, this list of conditions and the following disclaimer in the
14// documentation and/or other materials provided with the distribution.
15//
16// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
17// "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
18// LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
19// A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
20// HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
21// SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
22// LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
23// DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
24// THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25// (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
26// OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH
27// DAMAGE.
28
29#ifndef HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH
30#define HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH
31
32#include <hpp/core/fwd.hh>
33#include <hpp/util/debug.hh>
34
35namespace hpp {
36namespace core {
37namespace pathOptimization {
39 struct LinearConstraint {
40 LinearConstraint(size_type inputSize, size_type outputSize)
41 : J(outputSize, inputSize), b(outputSize), xSol(inputSize) {
42 J.setZero();
43 b.setZero();
44 }
45
46 ~LinearConstraint();
47
48 void concatenate(const LinearConstraint& oc) {
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;
54 }
55
61 bool decompose(bool check = false, bool throwIfNotValid = false);
62
64 void computeRank() {
65 if (J.size() == 0)
66 rank = 0;
67 else {
68 Eigen::FullPivLU<matrix_t> lu(J);
69 rank = lu.rank();
70 }
71 }
72
80 bool reduceConstraint(const LinearConstraint& lc, LinearConstraint& lcr,
81 bool computeRank = true) const {
82 lcr.J.noalias() = lc.J * PK;
83 lcr.b.noalias() = lc.b - lc.J * xStar;
84
85 // Decompose
86 if (computeRank) {
87 lcr.computeRank();
88 return lcr.rank == std::min(lcr.J.rows(), lcr.J.cols());
89 } else
90 return true;
91 }
92
97 void computeSolution(const vector_t& v) {
98 xSol.noalias() = xStar + PK * v;
99#ifdef HPP_DEBUG
100 isSatisfied(xSol);
101#endif // HPP_DEBUG
102 }
103
105 bool isSatisfied(const vector_t& x,
106 const value_type& threshold =
107 Eigen::NumTraits<value_type>::dummy_precision()) {
108 vector_t err(J * x - b);
109 if (err.isZero(threshold)) return true;
110 hppDout(error, "constraints could not be satisfied: " << err.norm() << '\n'
111 << err);
112 return false;
113 }
114
115 void addRows(const std::size_t& nbRows) {
116 if (nbRows > 0) {
117 J.conservativeResize(J.rows() + nbRows, J.cols());
118 b.conservativeResize(b.rows() + nbRows);
119
120 J.bottomRows(nbRows).setZero();
121 }
122 }
123
126 matrix_t J;
127 vector_t b;
129
134
136 size_type rank;
137
139 matrix_t PK;
141 vector_t xStar, xSol;
142
144};
145} // namespace pathOptimization
146} // namespace core
147} // namespace hpp
148
149#endif // HPP_CORE_PATH_OPTIMIZATION_SPLINE_GRADIENT_BASED_LINEAR_CONSTRAINT_HH
debug.hh
fwd.hh
hppDout
#define hppDout(channel, data)
hpp
hpp::core::value_type
pinocchio::value_type value_type
Definition fwd.hh:174
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition fwd.hh:220
hpp::core::size_type
pinocchio::size_type size_type
Definition fwd.hh:173
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition fwd.hh:162
hpp::core::pathOptimization::LinearConstraint
A linear constraint .
Definition linear-constraint.hh:39
hpp::core::pathOptimization::LinearConstraint::reduceConstraint
bool reduceConstraint(const LinearConstraint &lc, LinearConstraint &lcr, bool computeRank=true) const
Definition linear-constraint.hh:80
hpp::core::pathOptimization::LinearConstraint::LinearConstraint
LinearConstraint(size_type inputSize, size_type outputSize)
Definition linear-constraint.hh:40
hpp::core::pathOptimization::LinearConstraint::b
vector_t b
Definition linear-constraint.hh:127
hpp::core::pathOptimization::LinearConstraint::computeRank
void computeRank()
Compute rank of the constraint using a LU decomposition.
Definition linear-constraint.hh:64
hpp::core::pathOptimization::LinearConstraint::xSol
vector_t xSol
Definition linear-constraint.hh:141
hpp::core::pathOptimization::LinearConstraint::J
matrix_t J
Definition linear-constraint.hh:126
hpp::core::pathOptimization::LinearConstraint::PK
matrix_t PK
Projector onto .
Definition linear-constraint.hh:139
hpp::core::pathOptimization::LinearConstraint::xStar
vector_t xStar
is a particular solution.
Definition linear-constraint.hh:141
hpp::core::pathOptimization::LinearConstraint::rank
size_type rank
Rank of J.
Definition linear-constraint.hh:136
hpp::core::pathOptimization::LinearConstraint::decompose
bool decompose(bool check=false, bool throwIfNotValid=false)
hpp::core::pathOptimization::LinearConstraint::concatenate
void concatenate(const LinearConstraint &oc)
Definition linear-constraint.hh:48
hpp::core::pathOptimization::LinearConstraint::isSatisfied
bool isSatisfied(const vector_t &x, const value_type &threshold=Eigen::NumTraits< value_type >::dummy_precision())
Returns .
Definition linear-constraint.hh:105
hpp::core::pathOptimization::LinearConstraint::~LinearConstraint
~LinearConstraint()
hpp::core::pathOptimization::LinearConstraint::addRows
void addRows(const std::size_t &nbRows)
Definition linear-constraint.hh:115
hpp::core::pathOptimization::LinearConstraint::computeSolution
void computeSolution(const vector_t &v)
Definition linear-constraint.hh:97