hpp-core 4.14.0
Implement basic classes for canonical path planning for kinematic chains.
Loading...
Searching...
No Matches
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
35namespace hpp {
36namespace core {
39namespace 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
126
131 double solve(const LinearConstraint& ce, const LinearConstraint& ci);
132
134
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
void solve()
Definition: quadratic-program.hh:118
vector_t xStar
Definition: quadratic-program.hh:153
void reduced(const LinearConstraint &lc, QuadraticProgram &QPr) const
Definition: quadratic-program.hh:106
Eigen::LLT< matrix_t, Eigen::Lower > LLT_t
Definition: quadratic-program.hh:63
double solve(const LinearConstraint &ce, const LinearConstraint &ci)
bool bIsZero
Definition: quadratic-program.hh:139
value_type trace
Definition: quadratic-program.hh:145
Eigen::VectorXi activeConstraint
Definition: quadratic-program.hh:146
Eigen::JacobiSVD< matrix_t > Decomposition_t
Definition: quadratic-program.hh:62
QuadraticProgram(const QuadraticProgram &QP)
Definition: quadratic-program.hh:85
QuadraticProgram(const QuadraticProgram &QP, const LinearConstraint &lc)
Definition: quadratic-program.hh:75
Decomposition_t dec
Definition: quadratic-program.hh:152
QuadraticProgram(size_type inputSize)
Definition: quadratic-program.hh:65
vector_t b
Definition: quadratic-program.hh:138
void addRows(const std::size_t &nbRows)
Definition: quadratic-program.hh:90
matrix_t H
Definition: quadratic-program.hh:137
int activeSetSize
Definition: quadratic-program.hh:147
LLT_t llt
Definition: quadratic-program.hh:144
Definition: relative-motion.hh:115
pinocchio::value_type value_type
Definition: fwd.hh:173
pinocchio::vector_t vector_t
Definition: fwd.hh:219
pinocchio::size_type size_type
Definition: fwd.hh:172
pinocchio::matrix_t matrix_t
Definition: fwd.hh:161
Definition: bi-rrt-planner.hh:35
A linear constraint .
Definition: linear-constraint.hh:39
matrix_t PK
Projector onto .
Definition: linear-constraint.hh:139
vector_t xStar
is a particular solution.
Definition: linear-constraint.hh:141
Definition: quadratic-program.hh:61