GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
11			#define CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
12
13			#include "crocoddyl/core/utils/deprecate.hpp"
14			#include "crocoddyl/multibody/force-base.hpp"
15			#include "crocoddyl/multibody/fwd.hpp"
16			#include "crocoddyl/multibody/states/multibody.hpp"
17
18			namespace crocoddyl {
19
20			class ImpulseModelBase {
21			public:
22		✗	virtual ~ImpulseModelBase() = default;
23
24		✗	CROCODDYL_BASE_CAST(ImpulseModelBase, ImpulseModelAbstractTpl)
25			};
26
27			template <typename _Scalar>
28			class ImpulseModelAbstractTpl : public ImpulseModelBase {
29			public:
30			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
31
32			typedef _Scalar Scalar;
33			typedef MathBaseTpl<Scalar> MathBase;
34			typedef ImpulseDataAbstractTpl<Scalar> ImpulseDataAbstract;
35			typedef StateMultibodyTpl<Scalar> StateMultibody;
36			typedef typename MathBase::VectorXs VectorXs;
37			typedef typename MathBase::MatrixXs MatrixXs;
38
39			ImpulseModelAbstractTpl(std::shared_ptr<StateMultibody> state,
40			const pinocchio::ReferenceFrame type,
41			const std::size_t nc);
42
43			DEPRECATED(
44			"Use constructor that passes the type type of contact, this assumes is "
45			"pinocchio::LOCAL",
46			ImpulseModelAbstractTpl(std::shared_ptr<StateMultibody> state,
47			const std::size_t nc);)
48		✗	virtual ~ImpulseModelAbstractTpl() = default;
49
50			virtual void calc(const std::shared_ptr<ImpulseDataAbstract>& data,
51			const Eigen::Ref<const VectorXs>& x) = 0;
52			virtual void calcDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
53			const Eigen::Ref<const VectorXs>& x) = 0;
54
55			virtual void updateForce(const std::shared_ptr<ImpulseDataAbstract>& data,
56			const VectorXs& force) = 0;
57			void updateForceDiff(const std::shared_ptr<ImpulseDataAbstract>& data,
58			const MatrixXs& df_dx) const;
59			void setZeroForce(const std::shared_ptr<ImpulseDataAbstract>& data) const;
60			void setZeroForceDiff(const std::shared_ptr<ImpulseDataAbstract>& data) const;
61
62			virtual std::shared_ptr<ImpulseDataAbstract> createData(
63			pinocchio::DataTpl<Scalar>* const data);
64
65			const std::shared_ptr<StateMultibody>& get_state() const;
66			std::size_t get_nc() const;
67			DEPRECATED("Use get_nc().", std::size_t get_ni() const;)
68			std::size_t get_nu() const;
69
70			/**
71			* @brief Return the reference frame id
72			*/
73			pinocchio::FrameIndex get_id() const;
74
75			/**
76			* @brief Modify the reference frame id
77			*/
78			void set_id(const pinocchio::FrameIndex id);
79
80			/**
81			* @brief Modify the type of contact
82			*/
83			void set_type(const pinocchio::ReferenceFrame type);
84
85			/**
86			* @brief Return the type of contact
87			*/
88			pinocchio::ReferenceFrame get_type() const;
89
90			/**
91			* @brief Print information on the impulse model
92			*/
93			template <class Scalar>
94			friend std::ostream& operator<<(std::ostream& os,
95			const ImpulseModelAbstractTpl<Scalar>& model);
96
97			/**
98			* @brief Print relevant information of the impulse model
99			*
100			* @param[out] os Output stream object
101			*/
102			virtual void print(std::ostream& os) const;
103
104			protected:
105			std::shared_ptr<StateMultibody> state_;
106			std::size_t nc_;
107			pinocchio::FrameIndex id_; //!< Reference frame id of the contact
108			pinocchio::ReferenceFrame type_; //!< Type of contact
109		✗	ImpulseModelAbstractTpl() : state_(nullptr), nc_(0), id_(0) {};
110			};
111
112			template <typename _Scalar>
113			struct ImpulseDataAbstractTpl : public ForceDataAbstractTpl<_Scalar> {
114			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
115
116			typedef _Scalar Scalar;
117			typedef MathBaseTpl<Scalar> MathBase;
118			typedef ForceDataAbstractTpl<Scalar> Base;
119			typedef typename MathBase::VectorXs VectorXs;
120			typedef typename MathBase::MatrixXs MatrixXs;
121			typedef typename pinocchio::SE3Tpl<Scalar> SE3;
122
123			template <template <typename Scalar> class Model>
124		✗	ImpulseDataAbstractTpl(Model<Scalar>* const model,
125			pinocchio::DataTpl<Scalar>* const data)
126			: Base(model, data),
127		✗	dv0_dq(model->get_nc(), model->get_state()->get_nv()),
128		✗	dtau_dq(model->get_state()->get_nv(), model->get_state()->get_nv()) {
129		✗	dv0_dq.setZero();
130		✗	dtau_dq.setZero();
131		✗	}
132		✗	virtual ~ImpulseDataAbstractTpl() = default;
133
134			using Base::df_dx;
135			using Base::f;
136			using Base::frame;
137			using Base::Jc;
138			using Base::jMf;
139			using Base::pinocchio;
140
141			typename SE3::ActionMatrixType fXj;
142			MatrixXs dv0_dq;
143			MatrixXs dtau_dq;
144			};
145
146			} // namespace crocoddyl
147
148			/* --- Details -------------------------------------------------------------- */
149			/* --- Details -------------------------------------------------------------- */
150			/* --- Details -------------------------------------------------------------- */
151			#include "crocoddyl/multibody/impulse-base.hxx"
152
153			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ImpulseModelAbstractTpl)
154			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ImpulseDataAbstractTpl)
155
156			#endif // CROCODDYL_MULTIBODY_IMPULSE_BASE_HPP_
157