contact-force.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_RESIDUALS_CONTACT_FORCE_HPP_
#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_

#include "crocoddyl/core/residual-base.hpp"
#include "crocoddyl/multibody/contact-base.hpp"
#include "crocoddyl/multibody/contacts/contact-1d.hpp"
#include "crocoddyl/multibody/contacts/contact-3d.hpp"
#include "crocoddyl/multibody/contacts/contact-6d.hpp"
#include "crocoddyl/multibody/contacts/multiple-contacts.hpp"
#include "crocoddyl/multibody/data/contacts.hpp"
#include "crocoddyl/multibody/data/impulses.hpp"
#include "crocoddyl/multibody/fwd.hpp"
#include "crocoddyl/multibody/impulse-base.hpp"
#include "crocoddyl/multibody/impulses/impulse-3d.hpp"
#include "crocoddyl/multibody/impulses/impulse-6d.hpp"
#include "crocoddyl/multibody/impulses/multiple-impulses.hpp"
#include "crocoddyl/multibody/states/multibody.hpp"

namespace crocoddyl {

template <typename _Scalar>
class ResidualModelContactForceTpl : public ResidualModelAbstractTpl<_Scalar> {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelContactForceTpl)

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ResidualModelAbstractTpl<Scalar> Base;
 typedef ResidualDataContactForceTpl<Scalar> Data;
 typedef StateMultibodyTpl<Scalar> StateMultibody;
 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
 typedef pinocchio::ForceTpl<Scalar> Force;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;

 ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state,
 const pinocchio::FrameIndex id,
 const Force& fref, const std::size_t nc,
 const std::size_t nu, const bool fwddyn = true);

 ResidualModelContactForceTpl(std::shared_ptr<StateMultibody> state,
 const pinocchio::FrameIndex id,
 const Force& fref, const std::size_t nc);
 virtual ~ResidualModelContactForceTpl() = default;

 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& x,
 const Eigen::Ref<const VectorXs>& u) override;

 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& x) override;

 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& x,
 const Eigen::Ref<const VectorXs>& u) override;

 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
 const Eigen::Ref<const VectorXs>& x) override;

 virtual std::shared_ptr<ResidualDataAbstract> createData(
 DataCollectorAbstract* const data) override;

 void updateJacobians(const std::shared_ptr<ResidualDataAbstract>& data);

 template <typename NewScalar>
 ResidualModelContactForceTpl<NewScalar> cast() const;

 bool is_fwddyn() const;

 pinocchio::FrameIndex get_id() const;

 const Force& get_reference() const;

 DEPRECATED("Do not use set_id, instead create a new model",
 void set_id(const pinocchio::FrameIndex id);)

 void set_reference(const Force& reference);

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

 protected:
 using Base::nr_;
 using Base::nu_;
 using Base::state_;

 private:
 bool fwddyn_;
 bool update_jacobians_;
 pinocchio::FrameIndex id_;
 Force fref_;
};

template <typename _Scalar>
struct ResidualDataContactForceTpl : public ResidualDataAbstractTpl<_Scalar> {
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ResidualDataAbstractTpl<Scalar> Base;
 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
 typedef ContactModelMultipleTpl<Scalar> ContactModelMultiple;
 typedef ImpulseModelMultipleTpl<Scalar> ImpulseModelMultiple;
 typedef pinocchio::ForceTpl<Scalar> Force;
 typedef StateMultibodyTpl<Scalar> StateMultibody;
 typedef typename MathBase::MatrixXs MatrixXs;

 template <template <typename Scalar> class Model>
 ResidualDataContactForceTpl(Model<Scalar>* const model,
 DataCollectorAbstract* const data)
 : Base(model, data) {
 contact_type = ContactUndefined;

 // Check that proper shared data has been passed
 bool is_contact = true;
 DataCollectorContactTpl<Scalar>* d1 =
 dynamic_cast<DataCollectorContactTpl<Scalar>*>(shared);
 DataCollectorImpulseTpl<Scalar>* d2 =
 dynamic_cast<DataCollectorImpulseTpl<Scalar>*>(shared);
 if (d1 == NULL && d2 == NULL) {
 throw_pretty(
 "Invalid argument: the shared data should be derived from "
 "DataCollectorContact or DataCollectorImpulse");
 }
 if (d2 != NULL) {
 is_contact = false;
 }

 // Avoids data casting at runtime
 const pinocchio::FrameIndex id = model->get_id();
 const std::shared_ptr<StateMultibody>& state =
 std::static_pointer_cast<StateMultibody>(model->get_state());
 std::string frame_name = state->get_pinocchio()->frames[id].name;
 bool found_contact = false;
 if (is_contact) {
 for (typename ContactModelMultiple::ContactDataContainer::iterator it =
 d1->contacts->contacts.begin();
 it != d1->contacts->contacts.end(); ++it) {
 if (it->second->frame == id) {
 ContactData1DTpl<Scalar>* d1d =
 dynamic_cast<ContactData1DTpl<Scalar>*>(it->second.get());
 if (d1d != NULL) {
 contact_type = Contact1D;
 found_contact = true;
 contact = it->second;
 break;
 }
 ContactData3DTpl<Scalar>* d3d =
 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
 if (d3d != NULL) {
 contact_type = Contact3D;
 found_contact = true;
 contact = it->second;
 break;
 }
 ContactData6DTpl<Scalar>* d6d =
 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
 if (d6d != NULL) {
 contact_type = Contact6D;
 found_contact = true;
 contact = it->second;
 break;
 }
 throw_pretty(
 "Domain error: there isn't defined at least a 3d contact for " +
 frame_name);
 break;
 }
 }
 } else {
 for (typename ImpulseModelMultiple::ImpulseDataContainer::iterator it =
 d2->impulses->impulses.begin();
 it != d2->impulses->impulses.end(); ++it) {
 if (it->second->frame == id) {
 ImpulseData3DTpl<Scalar>* d3d =
 dynamic_cast<ImpulseData3DTpl<Scalar>*>(it->second.get());
 if (d3d != NULL) {
 contact_type = Contact3D;
 found_contact = true;
 contact = it->second;
 break;
 }
 ImpulseData6DTpl<Scalar>* d6d =
 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
 if (d6d != NULL) {
 contact_type = Contact6D;
 found_contact = true;
 contact = it->second;
 break;
 }
 throw_pretty(
 "Domain error: there isn't defined at least a 3d impulse for " +
 frame_name);
 break;
 }
 }
 }
 if (!found_contact) {
 throw_pretty(
 "Domain error: there isn't defined contact/impulse data for " +
 frame_name);
 }
 }
 virtual ~ResidualDataContactForceTpl() = default;

 std::shared_ptr<ForceDataAbstractTpl<Scalar> >
 contact;
 ContactType contact_type;
 using Base::r;
 using Base::Ru;
 using Base::Rx;
 using Base::shared;
};

} // namespace crocoddyl

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

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelContactForceTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataContactForceTpl)

#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_FORCE_HPP_