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__