GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-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_RESIDUALS_IMPULSE_COM_HPP_
11			#define CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
12
13			#include "crocoddyl/core/residual-base.hpp"
14			#include "crocoddyl/multibody/data/impulses.hpp"
15			#include "crocoddyl/multibody/fwd.hpp"
16			#include "crocoddyl/multibody/impulse-base.hpp"
17			#include "crocoddyl/multibody/states/multibody.hpp"
18
19			namespace crocoddyl {
20
21			/**
22			* @brief Impulse CoM residual
23			*
24			* This residual function is defined as
25			* \f$\mathbf{r}=\mathbf{J}_{com}*(\mathbf{v}_{next}-\mathbf{v})\f$,
26			* \f$\mathbf{J}_{com}\in\mathbb{R}^{3\times nv}\f$ is the CoM Jacobian, and
27			* \f$\mathbf{v}_{next},\mathbf{v}\in T_{\mathbf{q}}\mathcal{Q}\f$ are the
28			* generalized velocities after and before the impulse, respectively. Note that
29			* the dimension of the residual vector is 3. Furthermore, the Jacobians of the
30			* residual function are computed analytically.
31			*
32			* As described in `ResidualModelAbstractTpl()`, the residual value and its
33			* Jacobians are calculated by `calc` and `calcDiff`, respectively.
34			*
35			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
36			*/
37			template <typename _Scalar>
38			class ResidualModelImpulseCoMTpl : public ResidualModelAbstractTpl<_Scalar> {
39			public:
40			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41		✗	CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelImpulseCoMTpl)
42
43			typedef _Scalar Scalar;
44			typedef MathBaseTpl<Scalar> MathBase;
45			typedef ResidualModelAbstractTpl<Scalar> Base;
46			typedef ResidualDataImpulseCoMTpl<Scalar> Data;
47			typedef StateMultibodyTpl<Scalar> StateMultibody;
48			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
49			typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
50			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
51			typedef typename MathBase::VectorXs VectorXs;
52
53			/**
54			* @brief Initialize the impulse CoM residual model
55			*
56			* @param[in] state State of the multibody system
57			*/
58			ResidualModelImpulseCoMTpl(std::shared_ptr<StateMultibody> state);
59		✗	virtual ~ResidualModelImpulseCoMTpl() = default;
60
61			/**
62			* @brief Compute the impulse CoM residual
63			*
64			* @param[in] data Impulse CoM residual data
65			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
66			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
67			*/
68			virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
69			const Eigen::Ref<const VectorXs>& x,
70			const Eigen::Ref<const VectorXs>& u) override;
71
72			/**
73			* @brief Compute the Jacobians of the impulse CoM residual
74			*
75			* @param[in] data Impulse CoM residual data
76			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
77			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
78			*/
79			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
80			const Eigen::Ref<const VectorXs>& x,
81			const Eigen::Ref<const VectorXs>& u) override;
82
83			/**
84			* @brief Create the impulse CoM residual data
85			*/
86			virtual std::shared_ptr<ResidualDataAbstract> createData(
87			DataCollectorAbstract* const data) override;
88
89			/**
90			* @brief Cast the impulse-com residual model to a different scalar type.
91			*
92			* It is useful for operations requiring different precision or scalar types.
93			*
94			* @tparam NewScalar The new scalar type to cast to.
95			* @return ResidualModelImpulseCoMTpl<NewScalar> A residual model with the
96			* new scalar type.
97			*/
98			template <typename NewScalar>
99			ResidualModelImpulseCoMTpl<NewScalar> cast() const;
100
101			/**
102			* @brief Print relevant information of the impulse-com residual
103			*
104			* @param[out] os Output stream object
105			*/
106			virtual void print(std::ostream& os) const override;
107
108			protected:
109			using Base::nu_;
110			using Base::state_;
111			using Base::u_dependent_;
112
113			private:
114			std::shared_ptr<typename StateMultibody::PinocchioModel> pin_model_;
115			};
116
117			template <typename _Scalar>
118			struct ResidualDataImpulseCoMTpl : public ResidualDataAbstractTpl<_Scalar> {
119			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
120
121			typedef _Scalar Scalar;
122			typedef MathBaseTpl<Scalar> MathBase;
123			typedef ResidualDataAbstractTpl<Scalar> Base;
124			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
125			typedef StateMultibodyTpl<Scalar> StateMultibody;
126			typedef typename MathBase::MatrixXs MatrixXs;
127			typedef typename MathBase::Matrix3xs Matrix3xs;
128
129			template <template <typename Scalar> class Model>
130		✗	ResidualDataImpulseCoMTpl(Model<Scalar>* const model,
131			DataCollectorAbstract* const data)
132			: Base(model, data),
133		✗	dvc_dq(3, model->get_state()->get_nv()),
134		✗	ddv_dv(model->get_state()->get_nv(), model->get_state()->get_nv()) {
135		✗	dvc_dq.setZero();
136		✗	ddv_dv.setZero();
137		✗	const std::shared_ptr<StateMultibody>& state =
138			std::static_pointer_cast<StateMultibody>(model->get_state());
139		✗	pinocchio_internal =
140		✗	pinocchio::DataTpl<Scalar>(*state->get_pinocchio().get());
141			// Check that proper shared data has been passed
142		✗	DataCollectorMultibodyInImpulseTpl<Scalar>* d =
143		✗	dynamic_cast<DataCollectorMultibodyInImpulseTpl<Scalar>*>(shared);
144		✗	if (d == NULL) {
145		✗	throw_pretty(
146			"Invalid argument: the shared data should be derived from "
147			"DataCollectorMultibodyInImpulse");
148			}
149		✗	pinocchio = d->pinocchio;
150		✗	impulses = d->impulses;
151		✗	}
152		✗	virtual ~ResidualDataImpulseCoMTpl() = default;
153
154			pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
155			std::shared_ptr<crocoddyl::ImpulseDataMultipleTpl<Scalar> >
156			impulses; //!< Impulses data
157			Matrix3xs dvc_dq; //!< Jacobian of the CoM velocity
158			MatrixXs ddv_dv; //!< Jacobian of the CoM velocity
159			pinocchio::DataTpl<Scalar>
160			pinocchio_internal; //!< Pinocchio data for internal computation
161			using Base::r;
162			using Base::Ru;
163			using Base::Rx;
164			using Base::shared;
165			};
166
167			} // namespace crocoddyl
168
169			/* --- Details -------------------------------------------------------------- */
170			/* --- Details -------------------------------------------------------------- */
171			/* --- Details -------------------------------------------------------------- */
172			#include "crocoddyl/multibody/residuals/impulse-com.hxx"
173
174			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelImpulseCoMTpl)
175			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataImpulseCoMTpl)
176
177			#endif // CROCODDYL_MULTIBODY_RESIDUALS_IMPULSE_COM_HPP_
178