contact-base.hpp

// BSD 3-Clause License
//
// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
// Heriot-Watt University
// Copyright note valid unless otherwise stated in individual files.
// All rights reserved.

#ifndef CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_
#define CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_

#include "crocoddyl/core/utils/deprecate.hpp"
#include "crocoddyl/multibody/force-base.hpp"
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

namespace crocoddyl {

class ContactModelBase {
 public:
 virtual ~ContactModelBase() = default;

 CROCODDYL_BASE_CAST(ContactModelBase, ContactModelAbstractTpl)
};

template <typename _Scalar>
class ContactModelAbstractTpl : public ContactModelBase {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ContactDataAbstractTpl<Scalar> ContactDataAbstract;
 typedef StateMultibodyTpl<Scalar> StateMultibody;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
 const pinocchio::ReferenceFrame type,
 const std::size_t nc, const std::size_t nu);
 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
 const pinocchio::ReferenceFrame type,
 const std::size_t nc);

 DEPRECATED(
 "Use constructor that passes the type type of contact, this assumes is "
 "pinocchio::LOCAL",
 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
 const std::size_t nc, const std::size_t nu);)
 DEPRECATED(
 "Use constructor that passes the type type of contact, this assumes is "
 "pinocchio::LOCAL",
 ContactModelAbstractTpl(std::shared_ptr<StateMultibody> state,
 const std::size_t nc);)
 virtual ~ContactModelAbstractTpl() = default;

 virtual void calc(const std::shared_ptr<ContactDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& x) = 0;

 virtual void calcDiff(const std::shared_ptr<ContactDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& x) = 0;

 virtual void updateForce(const std::shared_ptr<ContactDataAbstract>& data,
 const VectorXs& force) = 0;

 void updateForceDiff(const std::shared_ptr<ContactDataAbstract>& data,
 const MatrixXs& df_dx, const MatrixXs& df_du) const;

 void setZeroForce(const std::shared_ptr<ContactDataAbstract>& data) const;

 void setZeroForceDiff(const std::shared_ptr<ContactDataAbstract>& data) const;

 virtual std::shared_ptr<ContactDataAbstract> createData(
 pinocchio::DataTpl<Scalar>* const data);

 const std::shared_ptr<StateMultibody>& get_state() const;

 std::size_t get_nc() const;

 std::size_t get_nu() const;

 pinocchio::FrameIndex get_id() const;

 void set_id(const pinocchio::FrameIndex id);

 void set_type(const pinocchio::ReferenceFrame type);

 pinocchio::ReferenceFrame get_type() const;

 template <class Scalar>
 friend std::ostream& operator<<(std::ostream& os,
 const ContactModelAbstractTpl<Scalar>& model);

 virtual void print(std::ostream& os) const;

 protected:
 std::shared_ptr<StateMultibody> state_;
 std::size_t nc_;
 std::size_t nu_;
 pinocchio::FrameIndex id_;
 pinocchio::ReferenceFrame type_;
 ContactModelAbstractTpl() : state_(nullptr), nc_(0), nu_(0), id_(0) {};
};

template <typename _Scalar>
struct ContactDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ForceDataAbstractTpl<Scalar> Base;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;
 typedef typename pinocchio::SE3Tpl<Scalar> SE3;

 template <template <typename Scalar> class Model>
 ContactDataAbstractTpl(Model<Scalar>* const model,
 pinocchio::DataTpl<Scalar>* const data)
 : Base(model, data),
 fXj(jMf.inverse().toActionMatrix()),
 a0(model->get_nc()),
 da0_dx(model->get_nc(), model->get_state()->get_ndx()),
 dtau_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
 a0.setZero();
 da0_dx.setZero();
 dtau_dq.setZero();
 }
 virtual ~ContactDataAbstractTpl() = default;

 using Base::df_du;
 using Base::df_dx;
 using Base::f;
 using Base::frame;
 using Base::Jc;
 using Base::jMf;
 using Base::pinocchio;

 typename SE3::ActionMatrixType fXj;
 VectorXs a0;
 MatrixXs da0_dx;
 MatrixXs dtau_dq;
};

} // namespace crocoddyl

/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
/* --- Details -------------------------------------------------------------- */
#include "crocoddyl/multibody/contact-base.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ContactModelAbstractTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ContactDataAbstractTpl)

#endif // CROCODDYL_MULTIBODY_CONTACT_BASE_HPP_