contact-wrench-cone.hpp

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

#ifndef CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_WRENCH_CONE_HPP_
#define CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_WRENCH_CONE_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/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"
#include "crocoddyl/multibody/wrench-cone.hpp"

namespace crocoddyl {

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

 typedef _Scalar Scalar;
 typedef MathBaseTpl<Scalar> MathBase;
 typedef ResidualModelAbstractTpl<Scalar> Base;
 typedef ResidualDataContactWrenchConeTpl<Scalar> Data;
 typedef StateMultibodyTpl<Scalar> StateMultibody;
 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
 typedef WrenchConeTpl<Scalar> WrenchCone;
 typedef typename MathBase::VectorXs VectorXs;
 typedef typename MathBase::MatrixXs MatrixXs;
 typedef typename MathBase::MatrixX6s MatrixX6s;

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

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

 bool is_fwddyn() const;

 pinocchio::FrameIndex get_id() const;

 const WrenchCone& 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 WrenchCone& 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_;
 WrenchCone fref_;
};

template <typename _Scalar>
struct ResidualDataContactWrenchConeTpl
 : 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 StateMultibodyTpl<Scalar> StateMultibody;
 typedef typename MathBase::MatrixXs MatrixXs;

 template <template <typename Scalar> class Model>
 ResidualDataContactWrenchConeTpl(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 ContactModelMultiple::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 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) {
 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 ~ResidualDataContactWrenchConeTpl() = default;

 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-wrench-cone.hxx"

CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
 crocoddyl::ResidualModelContactWrenchConeTpl)
CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
 crocoddyl::ResidualDataContactWrenchConeTpl)

#endif // CROCODDYL_MULTIBODY_RESIDUALS_CONTACT_WRENCH_CONE_HPP_