contact-cop-position.hpp

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

#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_
#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_

#include "crocoddyl/core/residual-base.hpp"
#include "crocoddyl/multibody/contact-base.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/cop-support.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 ResidualModelContactCoPPositionTpl
 : public ResidualModelAbstractTpl<_Scalar> {
 public:
 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
 CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelContactCoPPositionTpl)

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ResidualModelAbstractTpl<Scalar> Base;
 typedef ResidualDataContactCoPPositionTpl<Scalar> Data;
 typedef StateMultibodyTpl<Scalar> StateMultibody;
 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
 typedef CoPSupportTpl<Scalar> CoPSupport;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;
 typedef typename MathBase::Matrix46s Matrix46;

 ResidualModelContactCoPPositionTpl(std::shared_ptr<StateMultibody> state,
 const pinocchio::FrameIndex id,
 const CoPSupport& cref,
 const std::size_t nu,
 const bool fwddyn = true);

 ResidualModelContactCoPPositionTpl(std::shared_ptr<StateMultibody> state,
 const pinocchio::FrameIndex id,
 const CoPSupport& cref);
 virtual ~ResidualModelContactCoPPositionTpl() = 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>
 ResidualModelContactCoPPositionTpl<NewScalar> cast() const;

 bool is_fwddyn() const;

 pinocchio::FrameIndex get_id() const;

 const CoPSupport& get_reference() const;

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

 void set_reference(const CoPSupport& reference);

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

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

 private:
 bool fwddyn_;
 bool update_jacobians_;
 pinocchio::FrameIndex id_;
 CoPSupport cref_;
};

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

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ResidualDataAbstractTpl<Scalar> Base;
 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
 typedef StateMultibodyTpl<Scalar> StateMultibody;
 typedef typename MathBase::MatrixXs MatrixXs;

 template <template <typename Scalar> class Model>
 ResidualDataContactCoPPositionTpl(Model<Scalar>* const model,
 DataCollectorAbstract* const data)
 : Base(model, data) {
 // 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 ContactModelMultipleTpl<
 Scalar>::ContactDataContainer::iterator it =
 d1->contacts->contacts.begin();
 it != d1->contacts->contacts.end(); ++it) {
 if (it->second->frame == id) {
 ContactData3DTpl<Scalar>* d3d =
 dynamic_cast<ContactData3DTpl<Scalar>*>(it->second.get());
 if (d3d != NULL) {
 found_contact = true;
 contact = it->second;
 throw_pretty(
 "Domain error: there isn't defined at least a 6d contact for " +
 frame_name);
 break;
 }
 ContactData6DTpl<Scalar>* d6d =
 dynamic_cast<ContactData6DTpl<Scalar>*>(it->second.get());
 if (d6d != NULL) {
 found_contact = true;
 contact = it->second;
 break;
 }
 throw_pretty(
 "Domain error: there isn't defined at least a 6d contact for " +
 frame_name);
 break;
 }
 }
 } else {
 for (typename ImpulseModelMultipleTpl<
 Scalar>::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) {
 found_contact = true;
 contact = it->second;
 throw_pretty(
 "Domain error: there isn't defined at least a 6d contact for " +
 frame_name);
 break;
 }
 ImpulseData6DTpl<Scalar>* d6d =
 dynamic_cast<ImpulseData6DTpl<Scalar>*>(it->second.get());
 if (d6d != NULL) {
 found_contact = true;
 contact = it->second;
 break;
 }
 throw_pretty(
 "Domain error: there isn't defined at least a 6d contact for " +
 frame_name);
 break;
 }
 }
 }
 if (!found_contact) {
 throw_pretty("Domain error: there isn't defined contact data for " +
 frame_name);
 }
 }
 virtual ~ResidualDataContactCoPPositionTpl() = default;

 pinocchio::DataTpl<Scalar>* pinocchio;
 std::shared_ptr<ForceDataAbstractTpl<Scalar> > contact;
 using Base::r;
 using Base::Ru;
 using Base::Rx;
 using Base::shared;
};

} // namespace crocoddyl

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

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
 crocoddyl::ResidualModelContactCoPPositionTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
 crocoddyl::ResidualDataContactCoPPositionTpl)

#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_COP_POSITION_HPP_