Commit 778acf00 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Move SplineGradientBased::QuadraticProblem into QuadraticProgram (external class)

parent ace0f4bb
......@@ -92,6 +92,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/core/path-optimization/gradient-based.hh
include/hpp/core/path-optimization/linear-constraint.hh
include/hpp/core/path-optimization/partial-shortcut.hh
include/hpp/core/path-optimization/quadratic-program.hh
include/hpp/core/path-optimization/config-optimization.hh
include/hpp/core/path-optimization/simple-time-parameterization.hh
include/hpp/core/path-optimization/spline-gradient-based.hh
......
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#ifndef HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH
# define HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH
#include <hpp/core/fwd.hh>
#include <hpp/core/path-optimization/linear-constraint.hh>
namespace hpp {
namespace core {
/// \addtogroup path_optimization
/// \{
namespace pathOptimization {
/*/ Quadratic program
*
* This class stores a quadratic cost defined by
* \f$ 0.5 * x^T H x + b^T x \f$ where \f$ (H, b) \f$ are the parameters.
*
* It can then solve the two following program:
* \li Program subject to linear equality constraints
* \f{eqnarray*}{
* min & 0.5 * x^T H x + b^T x \\
* & A_0 * x = b_0
* \f}
* This is done via \ref reduced, \ref decompose and \ref solve methods
* \li Program subject to linear equality and inequality constraints:
* \f{eqnarray*}{
* min & 0.5 * x^T H x + b^T x \\
* & A_0 * x = b_0 \\
* & A_1 * x \ge b_1
* \f}
* This is done via \ref computeLLT and \ref solve methods
* and uses quadprog
**/
struct QuadraticProgram
{
typedef Eigen::JacobiSVD < matrix_t > Decomposition_t;
typedef Eigen::LLT <matrix_t, Eigen::Lower> LLT_t;
QuadraticProgram (size_type inputSize) :
H (inputSize, inputSize), b (inputSize),
dec (inputSize, inputSize, Eigen::ComputeThinU | Eigen::ComputeThinV),
xStar (inputSize)
{
H.setZero();
b.setZero();
bIsZero = true;
}
QuadraticProgram (const QuadraticProgram& QP, const LinearConstraint& lc) :
H (lc.PK.cols(), lc.PK.cols()), b (lc.PK.cols()), bIsZero (false),
dec (lc.PK.cols(), lc.PK.cols(), Eigen::ComputeThinU | Eigen::ComputeThinV),
xStar (lc.PK.cols())
{
QP.reduced (lc, *this);
}
QuadraticProgram (const QuadraticProgram& QP) :
H (QP.H), b (QP.b), bIsZero (QP.bIsZero),
dec (QP.dec), xStar (QP.xStar)
{}
~QuadraticProgram ();
void addRows (const std::size_t& nbRows)
{
H.conservativeResize(H.rows() + nbRows, H.cols());
b.conservativeResize(b.rows() + nbRows, b.cols());
H.bottomRows(nbRows).setZero();
}
/// \name Program subject to linear equality constraints.
/// \{
/*/ Compute the problem
* \f{eqnarray*}{
* min & 0.5 * x^T H x + b^T x \\
* & lc.J * x = lc.b
* \f}
**/
void reduced (const LinearConstraint& lc, QuadraticProgram& QPr) const
{
matrix_t H_PK (H * lc.PK);
QPr.H.noalias() = lc.PK.transpose() * H_PK;
QPr.b.noalias() = H_PK.transpose() * lc.xStar;
if (!bIsZero) {
QPr.b.noalias() += lc.PK.transpose() * b;
}
QPr.bIsZero = false;
}
void decompose ();
void solve ()
{
xStar.noalias() = - dec.solve(b);
}
/// \}
/// \name Program subject to linear equality and inequality constraints.
/// \{
void computeLLT();
/// Compute solution using quadprog
/// \param ce equality constraints
/// \param ci inequality constraints: \f$ ci.J * x \ge ci.b \f$
/// \note \ref computeLLT must have been called before.
double solve(const LinearConstraint& ce, const LinearConstraint& ci);
/// \}
/// \name Model
/// \{
matrix_t H;
vector_t b;
bool bIsZero;
/// \}
/// \name Data (for inequality constraints)
/// \{
LLT_t llt;
value_type trace;
Eigen::VectorXi activeConstraint;
int activeSetSize;
/// \}
/// \name Data (for equality constraints)
/// \{
Decomposition_t dec;
vector_t xStar;
/// \}
};
} // namespace pathOptimization
} // namespace core
} // namespace hpp
#endif // HPP_CORE_PATH_OPTIMIZATION_QUADRATIC_PROGRAM_HH
......@@ -106,7 +106,6 @@ namespace hpp {
bool checkOptimum_;
private:
struct QuadraticProblem;
typedef std::vector <std::pair <CollisionPathValidationReportPtr_t,
std::size_t> > Reports_t;
struct CollisionFunctions;
......
......@@ -63,6 +63,7 @@ path-validations.cc
path-optimization/spline-gradient-based-abstract.cc #
path-optimization/spline-gradient-based.cc #
path-optimization/partial-shortcut.cc #
path-optimization/quadratic-program.cc
path-optimization/config-optimization.cc#
path-optimization/simple-time-parameterization.cc#
path-planner.cc #
......
// Copyright (c) 2018, Joseph Mirabel
// Authors: Joseph Mirabel (joseph.mirabel@laas.fr)
//
// This file is part of hpp-core.
// hpp-core is free software: you can redistribute it
// and/or modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation, either version
// 3 of the License, or (at your option) any later version.
//
// hpp-core is distributed in the hope that it will be
// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Lesser Public License for more details. You should have
// received a copy of the GNU Lesser General Public License along with
// hpp-core. If not, see <http://www.gnu.org/licenses/>.
#include <hpp/core/path-optimization/quadratic-program.hh>
#include <hpp/util/timer.hh>
#include <path-optimization/spline-gradient-based/eiquadprog_2011.hpp>
namespace hpp {
namespace core {
/// \addtogroup path_optimization
/// \{
namespace pathOptimization {
HPP_DEFINE_TIMECOUNTER(QuadraticProgram_decompose);
HPP_DEFINE_TIMECOUNTER(QuadraticProgram_computeLLT);
HPP_DEFINE_TIMECOUNTER(QuadraticProgram_solve_quadprog);
QuadraticProgram::~QuadraticProgram ()
{
HPP_DISPLAY_TIMECOUNTER(QuadraticProgram_decompose);
HPP_DISPLAY_TIMECOUNTER(QuadraticProgram_computeLLT);
HPP_DISPLAY_TIMECOUNTER(QuadraticProgram_solve_quadprog);
}
void QuadraticProgram::decompose ()
{
HPP_SCOPE_TIMECOUNTER(QuadraticProgram_decompose);
dec.compute(H);
assert(dec.rank() == H.rows());
}
void QuadraticProgram::computeLLT()
{
HPP_SCOPE_TIMECOUNTER(QuadraticProgram_computeLLT);
trace = H.trace();
llt.compute(H);
}
double QuadraticProgram::solve(const LinearConstraint& ce, const LinearConstraint& ci)
{
HPP_SCOPE_TIMECOUNTER(QuadraticProgram_solve_quadprog);
// min 0.5 * x G x + g0 x
// s.t. CE^T x + ce0 = 0
// CI^T x + ci0 >= 0
return solve_quadprog2 (llt, trace, b,
ce.J.transpose(), - ce.b,
ci.J.transpose(), - ci.b,
xStar, activeConstraint, activeSetSize);
}
} // namespace pathOptimization
} // namespace core
} // namespace hpp
......@@ -26,10 +26,10 @@
#include <hpp/core/config-projector.hh>
#include <hpp/core/problem.hh>
#include <hpp/core/collision-path-validation-report.hh>
#include <hpp/core/path-optimization/quadratic-program.hh>
#include <path-optimization/spline-gradient-based/cost.hh>
#include <path-optimization/spline-gradient-based/collision-constraint.hh>
#include <path-optimization/spline-gradient-based/eiquadprog_2011.hpp>
namespace hpp {
namespace core {
......@@ -42,9 +42,7 @@ namespace hpp {
typedef Eigen::BlockIndex BlockIndex;
HPP_DEFINE_TIMECOUNTER(SGB_qpDecomposition);
HPP_DEFINE_TIMECOUNTER(SGB_findNewConstraint);
HPP_DEFINE_TIMECOUNTER(SGB_qpSolve);
template <int NbRows>
VectorMap_t reshape (Eigen::Matrix<value_type, NbRows, Eigen::Dynamic, Eigen::RowMajor>& parameters)
......@@ -66,112 +64,6 @@ namespace hpp {
// ----------- Convenience class -------------------------------------- //
/** \f{eqnarray*}{
* min & 0.5 * x^T H x + b^T x \\
* & lc.J * x = lc.b
* \f}
**/
template <int _PB, int _SO>
struct SplineGradientBased<_PB, _SO>::QuadraticProblem
{
typedef Eigen::JacobiSVD < matrix_t > Decomposition_t;
typedef Eigen::LLT <matrix_t, Eigen::Lower> LLT_t;
QuadraticProblem (size_type inputSize) :
H (inputSize, inputSize), b (inputSize),
dec (inputSize, inputSize, Eigen::ComputeThinU | Eigen::ComputeThinV),
xStar (inputSize)
{
H.setZero();
b.setZero();
bIsZero = true;
}
QuadraticProblem (const QuadraticProblem& QP, const LinearConstraint& lc) :
H (lc.PK.cols(), lc.PK.cols()), b (lc.PK.cols()), bIsZero (false),
dec (lc.PK.cols(), lc.PK.cols(), Eigen::ComputeThinU | Eigen::ComputeThinV),
xStar (lc.PK.cols())
{
QP.reduced (lc, *this);
}
QuadraticProblem (const QuadraticProblem& QP) :
H (QP.H), b (QP.b), bIsZero (QP.bIsZero),
dec (QP.dec), xStar (QP.xStar)
{}
void addRows (const std::size_t& nbRows)
{
H.conservativeResize(H.rows() + nbRows, H.cols());
b.conservativeResize(b.rows() + nbRows, b.cols());
H.bottomRows(nbRows).setZero();
}
/*/ Compute the problem
* \f{eqnarray*}{
* min & 0.5 * x^T H x + b^T x \\
* & lc.J * x = lc.b
* \f}
**/
void reduced (const LinearConstraint& lc, QuadraticProblem& QPr) const
{
matrix_t H_PK (H * lc.PK);
QPr.H.noalias() = lc.PK.transpose() * H_PK;
QPr.b.noalias() = H_PK.transpose() * lc.xStar;
if (!bIsZero) {
QPr.b.noalias() += lc.PK.transpose() * b;
}
QPr.bIsZero = false;
}
void decompose ()
{
HPP_SCOPE_TIMECOUNTER(SGB_qpDecomposition);
dec.compute(H);
assert(dec.rank() == H.rows());
}
void solve ()
{
xStar.noalias() = - dec.solve(b);
}
void computeLLT()
{
HPP_SCOPE_TIMECOUNTER(SGB_qpDecomposition);
trace = H.trace();
llt.compute(H);
}
double solve(const LinearConstraint& ce, const LinearConstraint& ci)
{
HPP_SCOPE_TIMECOUNTER(SGB_qpSolve);
// min 0.5 * x G x + g0 x
// s.t. CE^T x + ce0 = 0
// CI^T x + ci0 >= 0
return solve_quadprog2 (llt, trace, b,
ce.J.transpose(), - ce.b,
ci.J.transpose(), - ci.b,
xStar, activeConstraint, activeSetSize);
}
// model
matrix_t H;
vector_t b;
bool bIsZero;
// Data
LLT_t llt;
value_type trace;
Eigen::VectorXi activeConstraint;
int activeSetSize;
// Data
Decomposition_t dec;
vector_t xStar;
};
/// TODO Two options:
/// - Split this class into two classes:
/// - Move generic part outside of this class
......@@ -542,7 +434,7 @@ namespace hpp {
Base::copy(splines, alphaSplines); Base::copy(splines, collSplines);
Reports_t reports;
QuadraticProblem QP(cost.inputDerivativeSize_);
QuadraticProgram QP(cost.inputDerivativeSize_);
value_type optimalCost, costLowerBound = 0;
cost.value(optimalCost, splines);
hppDout (info, "Initial cost is " << optimalCost);
......@@ -551,7 +443,7 @@ namespace hpp {
checkHessian(cost, QP.H, splines);
#endif // NDEBUG
QuadraticProblem QPc (QP, constraint);
QuadraticProgram QPc (QP, constraint);
QPc.computeLLT();
QPc.solve(collisionReduced, boundConstraintReduced);
......@@ -660,9 +552,7 @@ namespace hpp {
}
// 7
HPP_DISPLAY_TIMECOUNTER(SGB_qpDecomposition);
HPP_DISPLAY_TIMECOUNTER(SGB_findNewConstraint);
HPP_DISPLAY_TIMECOUNTER(SGB_qpSolve);
return this->buildPathVector (splines);
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment