contact-cholesky.hpp

//
// Copyright (c) 2020-2024 INRIA
//

#ifndef __pinocchio_python_algorithm_contact_cholesky_hpp__
#define __pinocchio_python_algorithm_contact_cholesky_hpp__

#include <eigenpy/memory.hpp>
#include "pinocchio/algorithm/contact-cholesky.hpp"

#include "pinocchio/algorithm/delassus-operator-dense.hpp"
#include "pinocchio/algorithm/delassus-operator-sparse.hpp"

#include "pinocchio/bindings/python/utils/macros.hpp"
#include "pinocchio/bindings/python/utils/std-vector.hpp"
#include "pinocchio/bindings/python/utils/comparable.hpp"
#include "pinocchio/bindings/python/utils/copyable.hpp"
#include "pinocchio/bindings/python/utils/model-checker.hpp"

#include "pinocchio/bindings/python/algorithm/delassus-operator.hpp"

namespace pinocchio
{
 namespace python
 {
 namespace bp = boost::python;

 template<typename ContactCholeskyDecomposition>
 struct ContactCholeskyDecompositionPythonVisitor
 : public boost::python::def_visitor<
 ContactCholeskyDecompositionPythonVisitor<ContactCholeskyDecomposition>>
 {
 typedef ContactCholeskyDecomposition Self;
 typedef typename ContactCholeskyDecomposition::Scalar Scalar;
 typedef typename ContactCholeskyDecomposition::RigidConstraintModel RigidConstraintModel;
 typedef typename ContactCholeskyDecomposition::RigidConstraintData RigidConstraintData;
 typedef typename ContactCholeskyDecomposition::Matrix Matrix;
 typedef typename ContactCholeskyDecomposition::Vector Vector;
 typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel)
 RigidConstraintModelVector;
 typedef typename PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData)
 RigidConstraintDataVector;

 typedef pinocchio::python::context::Model Model;
 typedef pinocchio::python::context::Data Data;

 template<class PyClass>
 void visit(PyClass & cl) const
 {
 cl.def(bp::init<>(bp::arg("self"), "Default constructor."))
 .def(bp::init<const Model &>(
 bp::args("self", "model"),
 "Constructor from a model.")[mimic_not_supported_function<>(1)])
 .def(bp::init<const Model &, const RigidConstraintModelVector &>(
 (bp::arg("self"), bp::arg("model"), bp::arg("contact_models")),
 "Constructor from a model and a collection of RigidConstraintModels.")
 [mimic_not_supported_function<>(1)])

 .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self, U, "")
 .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self, D, "")
 .PINOCCHIO_ADD_PROPERTY_READONLY_BYVALUE(Self, Dinv, "")

 .def("size", &Self::size, bp::arg("self"), "Size of the decomposition.")
 .def(
 "constraintDim", &Self::constraintDim, bp::arg("self"),
 "Returns the total dimension of the constraints contained in the Cholesky "
 "factorization.")
 .def(
 "numContacts", &Self::numContacts, bp::arg("self"),
 "Returns the number of contacts associated to this decomposition.")
 .def(
 "matrix", (Matrix(Self::*)(void) const)&Self::matrix, bp::arg("self"),
 "Returns the matrix resulting from the decomposition.")

 .def(
 "compute",
 (void (*)(
 Self & self, const Model &, Data &, const RigidConstraintModelVector &,
 RigidConstraintDataVector &, const Scalar))&compute,
 (bp::arg("self"), bp::arg("model"), bp::arg("data"), bp::arg("contact_models"),
 bp::arg("contact_datas"), bp::arg("mu") = 0),
 "Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix\n"
 "related to the system mass matrix and the Jacobians of the contact patches contained "
 "in\n"
 "the vector of RigidConstraintModel named contact_models. The decomposition is "
 "regularized with a factor mu.",
 mimic_not_supported_function<>(1))

 .def(
 "compute",
 (void (*)(
 Self & self, const Model &, Data &, const RigidConstraintModelVector &,
 RigidConstraintDataVector &, const Vector &))&compute,
 (bp::arg("self"), bp::arg("model"), bp::arg("data"), bp::arg("contact_models"),
 bp::arg("contact_datas"), bp::arg("mus")),
 "Computes the Cholesky decompostion of the augmented matrix containing the KKT matrix\n"
 "related to the system mass matrix and the Jacobians of the contact patches contained "
 "in\n"
 "the vector of RigidConstraintModel named contact_models. The decomposition is "
 "regularized with a factor mu.",
 mimic_not_supported_function<>(1))

 .def(
 "updateDamping", (void(Self::*)(const Scalar &)) & Self::updateDamping,
 bp::args("self", "mu"),
 "Update the damping term on the upper left block part of the KKT matrix. The "
 "damping term should be positive.")

 .def(
 "updateDamping", &Self::template updateDamping<Vector>, bp::args("self", "mus"),
 "Update the damping terms on the upper left block part of the KKT matrix. The "
 "damping terms should be all positives.")

 .def(
 "getInverseOperationalSpaceInertiaMatrix",
 (Matrix(Self::*)(void) const)&Self::getInverseOperationalSpaceInertiaMatrix,
 bp::arg("self"),
 "Returns the Inverse of the Operational Space Inertia Matrix resulting from the "
 "decomposition.",
 bp::return_value_policy<bp::return_by_value>())

 .def(
 "getOperationalSpaceInertiaMatrix",
 (Matrix(Self::*)(void) const)&Self::getOperationalSpaceInertiaMatrix, bp::arg("self"),
 "Returns the Operational Space Inertia Matrix resulting from the decomposition.",
 bp::return_value_policy<bp::return_by_value>())

 .def(
 "getInverseMassMatrix", (Matrix(Self::*)(void) const)&Self::getInverseMassMatrix,
 bp::arg("self"),
 "Returns the inverse of the Joint Space Inertia Matrix or \"mass matrix\".",
 bp::return_value_policy<bp::return_by_value>())

 .def(
 "solve", &solve<Matrix>, bp::args("self", "matrix"),
 "Computes the solution of \f$ A x = b \f$ where self corresponds to the Cholesky "
 "decomposition of A.",
 bp::return_value_policy<bp::return_by_value>())

 .def(
 "inverse", (Matrix(Self::*)(void) const)&Self::inverse, bp::arg("self"),
 "Returns the inverse matrix resulting from the decomposition.")

 .def(
 "getMassMatrixChoeslkyDecomposition",
 &Self::template getMassMatrixChoeslkyDecomposition<
 Scalar, 0, JointCollectionDefaultTpl>,
 bp::arg("self"),
 "Retrieves the Cholesky decomposition of the Mass Matrix contained in the current "
 "decomposition.")

 .def(
 "getDelassusCholeskyExpression", &Self::getDelassusCholeskyExpression, bp::arg("self"),
 "Returns the Cholesky decomposition expression associated to the underlying "
 "Delassus matrix.",
 bp::with_custodian_and_ward_postcall<0, 1>())

 .def(ComparableVisitor<Self, pinocchio::is_floating_point<Scalar>::value>());
 }

 static void expose()
 {
 bp::class_<ContactCholeskyDecomposition>(
 "ContactCholeskyDecomposition",
 "Contact information container for contact dynamic algorithms.", bp::no_init)
 .def(ContactCholeskyDecompositionPythonVisitor<ContactCholeskyDecomposition>())
 .def(CopyableVisitor<ContactCholeskyDecomposition>());

 {
 typedef typename ContactCholeskyDecomposition::DelassusCholeskyExpression
 DelassusCholeskyExpression;

 bp::class_<DelassusCholeskyExpression>(
 "DelassusCholeskyExpression",
 "Delassus Cholesky expression associated to a "
 "given ContactCholeskyDecomposition object.",
 bp::no_init)
 .def(bp::init<const ContactCholeskyDecomposition &>(
 bp::args("self", "cholesky_decomposition"),
 "Build from a given ContactCholeskyDecomposition object.")
 [bp::with_custodian_and_ward<1, 2>()])
 .def(
 "cholesky",
 +[](const DelassusCholeskyExpression & self) -> ContactCholeskyDecomposition & {
 return const_cast<ContactCholeskyDecomposition &>(self.cholesky());
 },
 bp::arg("self"),
 "Returns the Constraint Cholesky decomposition associated to this "
 "DelassusCholeskyExpression.",
 bp::return_internal_reference<>())
 .def(DelassusOperatorBasePythonVisitor<DelassusCholeskyExpression>());
 }

 {
#ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
 typedef DelassusOperatorDenseTpl<context::Scalar, context::Options> DelassusOperatorDense;
 bp::class_<DelassusOperatorDense>(
 "DelassusOperatorDense", "Delassus Cholesky dense operator from a dense matrix.",
 bp::no_init)
 .def(bp::init<const Eigen::Ref<const context::MatrixXs>>(
 bp::args("self", "matrix"), "Build from a given dense matrix"))

 .def(DelassusOperatorBasePythonVisitor<DelassusOperatorDense>());
#endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
 }

 {
#ifndef PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
 typedef DelassusOperatorSparseTpl<context::Scalar, context::Options>
 DelassusOperatorSparse;
 bp::class_<DelassusOperatorSparse, boost::noncopyable>(
 "DelassusOperatorSparse", "Delassus Cholesky sparse operator from a sparse matrix.",
 bp::no_init)
 .def(bp::init<const context::SparseMatrix &>(
 bp::args("self", "matrix"), "Build from a given sparse matrix"))

 .def(DelassusOperatorBasePythonVisitor<DelassusOperatorSparse>());
#endif // PINOCCHIO_PYTHON_SKIP_CASADI_UNSUPPORTED
 }
#ifdef PINOCCHIO_WITH_ACCELERATE_SUPPORT
 {
 typedef Eigen::AccelerateLLT<context::SparseMatrix> AccelerateCholeskyDecomposition;
 typedef DelassusOperatorSparseTpl<
 context::Scalar, context::Options, AccelerateCholeskyDecomposition>
 DelassusOperatorSparseAccelerate;
 bp::class_<DelassusOperatorSparseAccelerate, boost::noncopyable>(
 "DelassusOperatorSparseAccelerate",
 "Delassus Cholesky sparse operator leveraging the Accelerate framework on APPLE "
 "systems.",
 bp::no_init)
 .def(bp::init<const context::SparseMatrix &>(
 bp::args("self", "matrix"), "Build from a given sparse matrix"))

 .def(DelassusOperatorBasePythonVisitor<DelassusOperatorSparseAccelerate>());
 }
#endif
 }

 template<typename MatrixType>
 static Matrix solve(const Self & self, const MatrixType & mat)
 {
 return self.solve(mat);
 }

 static void compute(
 Self & self,
 const Model & model,
 Data & data,
 const RigidConstraintModelVector & contact_models,
 RigidConstraintDataVector & contact_datas,
 const Scalar mu = static_cast<Scalar>(0))
 {
 self.compute(model, data, contact_models, contact_datas, mu);
 }

 static void compute(
 Self & self,
 const Model & model,
 Data & data,
 const RigidConstraintModelVector & contact_models,
 RigidConstraintDataVector & contact_datas,
 const Vector & mus)
 {
 self.compute(model, data, contact_models, contact_datas, mus);
 }
 };

 } // namespace python
} // namespace pinocchio

#endif // ifndef __pinocchio_python_algorithm_contact_cholesky_hpp__