hpp-core  4.13.0
Implement basic classes for canonical path planning for kinematic chains.
quadratic-program.hh
Go to the documentation of this file.
1 // Copyright (c) 2018, 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_QUADRATIC_PROGRAM_HH
30 #define HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH
31 
32 #include <hpp/core/fwd.hh>
34 
35 namespace hpp {
36 namespace core {
39 namespace pathOptimization {
40 /*/ Quadratic program
41  *
42  * This class stores a quadratic cost defined by
43  * \f$ 0.5 * x^T H x + b^T x \f$ where \f$ (H, b) \f$ are the parameters.
44  *
45  * It can then solve the two following program:
46  * \li Program subject to linear equality constraints
47  * \f{eqnarray*}{
48  * min & 0.5 * x^T H x + b^T x \\
49  * & A_0 * x = b_0
50  * \f}
51  * This is done via \ref reduced, \ref decompose and \ref solve methods
52  * \li Program subject to linear equality and inequality constraints:
53  * \f{eqnarray*}{
54  * min & 0.5 * x^T H x + b^T x \\
55  * & A_0 * x = b_0 \\
56  * & A_1 * x \ge b_1
57  * \f}
58  * This is done via \ref computeLLT and \ref solve methods
59  * and uses quadprog
60  **/
62  typedef Eigen::JacobiSVD<matrix_t> Decomposition_t;
63  typedef Eigen::LLT<matrix_t, Eigen::Lower> LLT_t;
64 
66  : H(inputSize, inputSize),
67  b(inputSize),
68  dec(inputSize, inputSize, Eigen::ComputeThinU | Eigen::ComputeThinV),
69  xStar(inputSize) {
70  H.setZero();
71  b.setZero();
72  bIsZero = true;
73  }
74 
76  : H(lc.PK.cols(), lc.PK.cols()),
77  b(lc.PK.cols()),
78  bIsZero(false),
79  dec(lc.PK.cols(), lc.PK.cols(),
80  Eigen::ComputeThinU | Eigen::ComputeThinV),
81  xStar(lc.PK.cols()) {
82  QP.reduced(lc, *this);
83  }
84 
86  : H(QP.H), b(QP.b), bIsZero(QP.bIsZero), dec(QP.dec), xStar(QP.xStar) {}
87 
89 
90  void addRows(const std::size_t& nbRows) {
91  H.conservativeResize(H.rows() + nbRows, H.cols());
92  b.conservativeResize(b.rows() + nbRows, b.cols());
93 
94  H.bottomRows(nbRows).setZero();
95  }
96 
99 
100  /*/ Compute the problem
101  * \f{eqnarray*}{
102  * min & 0.5 * x^T H x + b^T x \\
103  * & lc.J * x = lc.b
104  * \f}
105  **/
106  void reduced(const LinearConstraint& lc, QuadraticProgram& QPr) const {
107  matrix_t H_PK(H * lc.PK);
108  QPr.H.noalias() = lc.PK.transpose() * H_PK;
109  QPr.b.noalias() = H_PK.transpose() * lc.xStar;
110  if (!bIsZero) {
111  QPr.b.noalias() += lc.PK.transpose() * b;
112  }
113  QPr.bIsZero = false;
114  }
115 
116  void decompose();
117 
118  void solve() { xStar.noalias() = -dec.solve(b); }
119 
121 
124 
125  void computeLLT();
126 
131  double solve(const LinearConstraint& ce, const LinearConstraint& ci);
132 
134 
139  bool bIsZero;
141 
146  Eigen::VectorXi activeConstraint;
149 
155 };
156 } // namespace pathOptimization
157 } // namespace core
158 } // namespace hpp
159 
160 #endif // HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH
hpp::core::pathOptimization::QuadraticProgram::bIsZero
bool bIsZero
Definition: quadratic-program.hh:139
hpp::core::size_type
pinocchio::size_type size_type
Definition: fwd.hh:162
Eigen
Definition: relative-motion.hh:115
hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram
QuadraticProgram(const QuadraticProgram &QP)
Definition: quadratic-program.hh:85
hpp::core::pathOptimization::QuadraticProgram::Decomposition_t
Eigen::JacobiSVD< matrix_t > Decomposition_t
Definition: quadratic-program.hh:62
hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram
QuadraticProgram(const QuadraticProgram &QP, const LinearConstraint &lc)
Definition: quadratic-program.hh:75
hpp::core::pathOptimization::QuadraticProgram::~QuadraticProgram
~QuadraticProgram()
hpp::core::pathOptimization::LinearConstraint::xStar
vector_t xStar
is a particular solution.
Definition: linear-constraint.hh:141
hpp::core::pathOptimization::LinearConstraint
A linear constraint .
Definition: linear-constraint.hh:39
hpp::core::vector_t
pinocchio::vector_t vector_t
Definition: fwd.hh:209
linear-constraint.hh
hpp::core::pathOptimization::QuadraticProgram::trace
value_type trace
Definition: quadratic-program.hh:145
hpp::core::pathOptimization::QuadraticProgram::LLT_t
Eigen::LLT< matrix_t, Eigen::Lower > LLT_t
Definition: quadratic-program.hh:63
hpp::core::pathOptimization::QuadraticProgram
Definition: quadratic-program.hh:61
fwd.hh
hpp::core::pathOptimization::QuadraticProgram::addRows
void addRows(const std::size_t &nbRows)
Definition: quadratic-program.hh:90
hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram
QuadraticProgram(size_type inputSize)
Definition: quadratic-program.hh:65
hpp::core::pathOptimization::QuadraticProgram::b
vector_t b
Definition: quadratic-program.hh:138
hpp
Definition: bi-rrt-planner.hh:35
hpp::core::pathOptimization::QuadraticProgram::xStar
vector_t xStar
Definition: quadratic-program.hh:153
hpp::core::pathOptimization::QuadraticProgram::solve
void solve()
Definition: quadratic-program.hh:118
hpp::core::matrix_t
pinocchio::matrix_t matrix_t
Definition: fwd.hh:151
hpp::core::pathOptimization::QuadraticProgram::decompose
void decompose()
hpp::core::pathOptimization::QuadraticProgram::H
matrix_t H
Definition: quadratic-program.hh:137
hpp::core::pathOptimization::QuadraticProgram::computeLLT
void computeLLT()
hpp::core::pathOptimization::QuadraticProgram::activeSetSize
int activeSetSize
Definition: quadratic-program.hh:147
hpp::core::pathOptimization::QuadraticProgram::activeConstraint
Eigen::VectorXi activeConstraint
Definition: quadratic-program.hh:146
hpp::core::pathOptimization::LinearConstraint::PK
matrix_t PK
Projector onto .
Definition: linear-constraint.hh:139
hpp::core::pathOptimization::QuadraticProgram::dec
Decomposition_t dec
Definition: quadratic-program.hh:152
hpp::core::pathOptimization::QuadraticProgram::reduced
void reduced(const LinearConstraint &lc, QuadraticProgram &QPr) const
Definition: quadratic-program.hh:106
hpp::core::pathOptimization::QuadraticProgram::llt
LLT_t llt
Definition: quadratic-program.hh:144
hpp::core::value_type
pinocchio::value_type value_type
Definition: fwd.hh:163