pgs-solver.hpp

//
// Copyright (c) 2022-2024 INRIA
//
 
#ifndef __pinocchio_algorithm_pgs_solver_hpp__
#define __pinocchio_algorithm_pgs_solver_hpp__
 
#include "pinocchio/algorithm/constraints/fwd.hpp"
#include "pinocchio/algorithm/contact-solver-base.hpp"
 
namespace pinocchio
{
 
  template<typename _Scalar>
  struct PGSContactSolverTpl : ContactSolverBaseTpl<_Scalar>
  {
    typedef _Scalar Scalar;
    typedef ContactSolverBaseTpl<Scalar> Base;
    typedef Eigen::Matrix<Scalar, Eigen::Dynamic, 1> VectorXs;
 
    explicit PGSContactSolverTpl(const int problem_size)
    : Base(problem_size)
    , x(problem_size)
    , x_previous(problem_size)
    {
    }
 
    template<
      typename MatrixLike,
      typename VectorLike,
      typename ConstraintAllocator,
      typename VectorLikeOut>
    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));
 
  protected:
    VectorXs x, x_previous;
#ifdef PINOCCHIO_WITH_HPP_FCL
    using Base::timer;
#endif // PINOCCHIO_WITH_HPP_FCL
 
  }; // struct PGSContactSolverTpl
} // namespace pinocchio
 
#include "pinocchio/algorithm/pgs-solver.hxx"
 
#endif // ifndef __pinocchio_algorithm_pgs_solver_hpp__