hpp-core: include/hpp/core/path-optimization/quadratic-program.hh Source File

hpp-core 9.0.2 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>
33#include <hpp/core/path-optimization/linear-constraint.hh>
34
35namespace hpp {
36namespace core {
39namespace pathOptimization {
61 struct QuadraticProgram {
62 typedef Eigen::JacobiSVD<matrix_t> Decomposition_t;
63 typedef Eigen::LLT<matrix_t, Eigen::Lower> LLT_t;
64
68 QuadraticProgram(size_type inputSize)
69 : H(inputSize, inputSize),
70 b(inputSize),
71 dec(inputSize, inputSize, Eigen::ComputeThinU | Eigen::ComputeThinV),
72 xStar(inputSize),
73 accuracy_(1e-4) {
74 H.setZero();
75 b.setZero();
76 bIsZero = true;
77 }
78
83 QuadraticProgram(const QuadraticProgram& QP, const LinearConstraint& lc)
84 : H(lc.PK.cols(), lc.PK.cols()),
85 b(lc.PK.cols()),
86 bIsZero(false),
87 dec(lc.PK.cols(), lc.PK.cols(),
88 Eigen::ComputeThinU | Eigen::ComputeThinV),
89 xStar(lc.PK.cols()),
90 accuracy_(1e-4) {
91 QP.reduced(lc, *this);
92 }
93
94 QuadraticProgram(const QuadraticProgram& QP)
95 : H(QP.H),
96 b(QP.b),
97 bIsZero(QP.bIsZero),
98 dec(QP.dec),
99 xStar(QP.xStar),
100 accuracy_(QP.accuracy_) {}
101
102 ~QuadraticProgram();
103
110 void accuracy(value_type acc) { accuracy_ = acc; }
116 value_type accuracy() const { return accuracy_; }
117 void addRows(const std::size_t& nbRows) {
118 H.conservativeResize(H.rows() + nbRows, H.cols());
119 b.conservativeResize(b.rows() + nbRows, b.cols());
120
121 H.bottomRows(nbRows).setZero();
122 }
123
126
127 /*/ Compute the problem
128 * \f{eqnarray*}{
129 * \min & \frac{1}{2} * x^T H x + b^T x \\
130 * lc.J * x = lc.b
131 * \f}
132 **/
133 void reduced(const LinearConstraint& lc, QuadraticProgram& QPr) const {
134 matrix_t H_PK(H * lc.PK);
135 QPr.H.noalias() = lc.PK.transpose() * H_PK;
136 QPr.b.noalias() = H_PK.transpose() * lc.xStar;
137 if (!bIsZero) {
138 QPr.b.noalias() += lc.PK.transpose() * b;
139 }
140 QPr.bIsZero = false;
141 }
142
143 void decompose();
144
145 void solve() { xStar.noalias() = -dec.solve(b); }
146
148
151
152 void computeLLT();
153
161 double solve(const LinearConstraint& ce, const LinearConstraint& ci);
162
164
167 matrix_t H;
168 vector_t b;
169 bool bIsZero;
171
174 LLT_t llt;
175 value_type trace;
176 Eigen::VectorXi activeConstraint;
177 int activeSetSize;
179
182 Decomposition_t dec;
183 vector_t xStar;
185 value_type accuracy_;
186};
187} // namespace pathOptimization
188} // namespace core
189} // namespace hpp
190
191#endif // HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH
fwd.hh
hpp::core::pathOptimization::QuadraticProgram::solve
void solve()
Definition quadratic-program.hh:145
hpp::core::pathOptimization::QuadraticProgram::xStar
vector_t xStar
Definition quadratic-program.hh:183
hpp::core::pathOptimization::QuadraticProgram::reduced
void reduced(const LinearConstraint &lc, QuadraticProgram &QPr) const
Definition quadratic-program.hh:133
hpp::core::pathOptimization::QuadraticProgram::LLT_t
Eigen::LLT< matrix_t, Eigen::Lower > LLT_t
Definition quadratic-program.hh:63
hpp::core::pathOptimization::QuadraticProgram::solve
double solve(const LinearConstraint &ce, const LinearConstraint &ci)
hpp::core::pathOptimization::QuadraticProgram::bIsZero
bool bIsZero
Definition quadratic-program.hh:169
hpp::core::pathOptimization::QuadraticProgram::trace
value_type trace
Definition quadratic-program.hh:175
hpp::core::pathOptimization::QuadraticProgram::accuracy
void accuracy(value_type acc)
Definition quadratic-program.hh:110
hpp::core::pathOptimization::QuadraticProgram::decompose
void decompose()
hpp::core::pathOptimization::QuadraticProgram::computeLLT
void computeLLT()
hpp::core::pathOptimization::QuadraticProgram::activeConstraint
Eigen::VectorXi activeConstraint
Definition quadratic-program.hh:176
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)
Definition quadratic-program.hh:94
hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram
QuadraticProgram(const QuadraticProgram &QP, const LinearConstraint &lc)
Definition quadratic-program.hh:83
hpp::core::pathOptimization::QuadraticProgram::dec
Decomposition_t dec
Definition quadratic-program.hh:182
hpp::core::pathOptimization::QuadraticProgram::~QuadraticProgram
~QuadraticProgram()
hpp::core::pathOptimization::QuadraticProgram::QuadraticProgram
QuadraticProgram(size_type inputSize)
Definition quadratic-program.hh:68
hpp::core::pathOptimization::QuadraticProgram::accuracy
value_type accuracy() const
Definition quadratic-program.hh:116
hpp::core::pathOptimization::QuadraticProgram::b
vector_t b
Definition quadratic-program.hh:168
hpp::core::pathOptimization::QuadraticProgram::accuracy_
value_type accuracy_
Definition quadratic-program.hh:185
hpp::core::pathOptimization::QuadraticProgram::addRows
void addRows(const std::size_t &nbRows)
Definition quadratic-program.hh:117
hpp::core::pathOptimization::QuadraticProgram::H
matrix_t H
Definition quadratic-program.hh:167
hpp::core::pathOptimization::QuadraticProgram::activeSetSize
int activeSetSize
Definition quadratic-program.hh:177
hpp::core::pathOptimization::QuadraticProgram::llt
LLT_t llt
Definition quadratic-program.hh:174
linear-constraint.hh
Eigen
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::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::QuadraticProgram
Definition quadratic-program.hh:61