pinocchio  3.2.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
pgs-solver.hpp
1 //
2 // Copyright (c) 2022-2024 INRIA
3 //
4 
5 #ifndef __pinocchio_algorithm_pgs_solver_hpp__
6 #define __pinocchio_algorithm_pgs_solver_hpp__
7 
8 #include "pinocchio/algorithm/constraints/fwd.hpp"
9 #include "pinocchio/algorithm/contact-solver-base.hpp"
10 
11 namespace pinocchio
12 {
13 
15  template<typename _Scalar>
17  {
18  typedef _Scalar Scalar;
20  typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorXs;
21 
22  explicit PGSContactSolverTpl(const int problem_size)
24  , x(problem_size)
25  , x_previous(problem_size)
26  {
27  }
28 
40  template<
41  typename MatrixLike,
42  typename VectorLike,
43  typename ConstraintAllocator,
44  typename VectorLikeOut>
45  bool solve(
46  const MatrixLike & G,
47  const Eigen::MatrixBase<VectorLike> & g,
48  const std::vector<CoulombFrictionConeTpl<Scalar>, ConstraintAllocator> & cones,
49  const Eigen::DenseBase<VectorLikeOut> & x,
50  const Scalar over_relax = Scalar(1));
51 
52  protected:
54  VectorXs x, x_previous;
55 #ifdef PINOCCHIO_WITH_HPP_FCL
56  using Base::timer;
57 #endif // PINOCCHIO_WITH_HPP_FCL
58 
59  }; // struct PGSContactSolverTpl
60 } // namespace pinocchio
61 
62 #include "pinocchio/algorithm/pgs-solver.hxx"
63 
64 #endif // ifndef __pinocchio_algorithm_pgs_solver_hpp__
Main pinocchio namespace.
Definition: treeview.dox:11
int problem_size
Size of the problem.
Projected Gauss Siedel solver.
Definition: pgs-solver.hpp:17
bool solve(const MatrixLike &G, const Eigen::MatrixBase< VectorLike > &g, const std::vector< CoulombFrictionConeTpl< Scalar >, ConstraintAllocator > &cones, const Eigen::DenseBase< VectorLikeOut > &x, const Scalar over_relax=Scalar(1))
Solve the constrained conic problem composed of problem data (G,g,cones) and starting from the initia...
VectorXs x
Previous temporary value of the optimum.
Definition: pgs-solver.hpp:54