GCC Code Coverage Report

Line	Branch	Exec	Source
1
2			///////////////////////////////////////////////////////////////////////////////
3			// BSD 3-Clause License
4			//
5			// Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh,
6			// Heriot-Watt University
7			// Copyright note valid unless otherwise stated in individual files.
8			// All rights reserved.
9			///////////////////////////////////////////////////////////////////////////////
10
11			#ifndef CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
12			#define CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
13
14			#include "crocoddyl/core/residual-base.hpp"
15			#include "crocoddyl/multibody/data/multibody.hpp"
16			#include "crocoddyl/multibody/fwd.hpp"
17			#include "crocoddyl/multibody/states/multibody.hpp"
18
19			namespace crocoddyl {
20
21			/**
22			* @brief Frame translation residual
23			*
24			* This residual function defines the tracking of a frame translation as
25			* \f$\mathbf{r}=\mathbf{t}-\mathbf{t}^*\f$, where
26			* \f$\mathbf{t},\mathbf{t}^*\in~\mathbb{R}^3\f$ are the current and reference
27			* frame translations, respectively. Note that the dimension of the residual
28			* vector is 3. Furthermore, the Jacobians of the residual function are computed
29			* analytically.
30			*
31			* As described in `ResidualModelAbstractTpl()`, the residual value and its
32			* Jacobians are calculated by `calc` and `calcDiff`, respectively.
33			*
34			* \sa `ResidualModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
35			*/
36			template <typename _Scalar>
37			class ResidualModelFrameTranslationTpl
38			: public ResidualModelAbstractTpl<_Scalar> {
39			public:
40			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41		✗	CROCODDYL_DERIVED_CAST(ResidualModelBase, ResidualModelFrameTranslationTpl)
42
43			typedef _Scalar Scalar;
44			typedef MathBaseTpl<Scalar> MathBase;
45			typedef ResidualModelAbstractTpl<Scalar> Base;
46			typedef ResidualDataFrameTranslationTpl<Scalar> Data;
47			typedef StateMultibodyTpl<Scalar> StateMultibody;
48			typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
49			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
50			typedef typename MathBase::VectorXs VectorXs;
51			typedef typename MathBase::Vector3s Vector3s;
52
53			/**
54			* @brief Initialize the frame translation residual model
55			*
56			* @param[in] state State of the multibody system
57			* @param[in] id Reference frame id
58			* @param[in] xref Reference frame translation
59			* @param[in] nu Dimension of the control vector
60			*/
61			ResidualModelFrameTranslationTpl(std::shared_ptr<StateMultibody> state,
62			const pinocchio::FrameIndex id,
63			const Vector3s& xref, const std::size_t nu);
64
65			/**
66			* @brief Initialize the frame translation residual model
67			*
68			* The default `nu` is equals to StateAbstractTpl::get_nv().
69			*
70			* @param[in] state State of the multibody system
71			* @param[in] id Reference frame id
72			* @param[in] xref Reference frame translation
73			*/
74			ResidualModelFrameTranslationTpl(std::shared_ptr<StateMultibody> state,
75			const pinocchio::FrameIndex id,
76			const Vector3s& xref);
77		✗	virtual ~ResidualModelFrameTranslationTpl() = default;
78
79			/**
80			* @brief Compute the frame translation residual
81			*
82			* @param[in] data Frame translation residual data
83			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
84			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
85			*/
86			virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
87			const Eigen::Ref<const VectorXs>& x,
88			const Eigen::Ref<const VectorXs>& u) override;
89
90			/**
91			* @brief Compute the derivatives of the frame translation residual
92			*
93			* @param[in] data Frame translation residual data
94			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
95			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
96			*/
97			virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
98			const Eigen::Ref<const VectorXs>& x,
99			const Eigen::Ref<const VectorXs>& u) override;
100
101			/**
102			* @brief Create the frame translation residual data
103			*/
104			virtual std::shared_ptr<ResidualDataAbstract> createData(
105			DataCollectorAbstract* const data) override;
106
107			/**
108			* @brief Cast the frame-translation residual model to a different scalar
109			* type.
110			*
111			* It is useful for operations requiring different precision or scalar types.
112			*
113			* @tparam NewScalar The new scalar type to cast to.
114			* @return ResidualModelFrameTranslationTpl<NewScalar> A residual model with
115			* the new scalar type.
116			*/
117			template <typename NewScalar>
118			ResidualModelFrameTranslationTpl<NewScalar> cast() const;
119
120			/**
121			* @brief Return the reference frame id
122			*/
123			pinocchio::FrameIndex get_id() const;
124
125			/**
126			* @brief Return the reference frame translation
127			*/
128			const Vector3s& get_reference() const;
129
130			/**
131			* @brief Modify the reference frame id
132			*/
133			void set_id(const pinocchio::FrameIndex id);
134
135			/**
136			* @brief Modify the reference frame translation reference
137			*/
138			void set_reference(const Vector3s& reference);
139
140			/**
141			* @brief Print relevant information of the frame-translation residual
142			*
143			* @param[out] os Output stream object
144			*/
145			virtual void print(std::ostream& os) const override;
146
147			protected:
148			using Base::nu_;
149			using Base::state_;
150			using Base::u_dependent_;
151			using Base::v_dependent_;
152
153			private:
154			pinocchio::FrameIndex id_; //!< Reference frame id
155			Vector3s xref_; //!< Reference frame translation
156			std::shared_ptr<typename StateMultibody::PinocchioModel>
157			pin_model_; //!< Pinocchio model
158			};
159
160			template <typename _Scalar>
161			struct ResidualDataFrameTranslationTpl
162			: public ResidualDataAbstractTpl<_Scalar> {
163			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
164
165			typedef _Scalar Scalar;
166			typedef MathBaseTpl<Scalar> MathBase;
167			typedef ResidualDataAbstractTpl<Scalar> Base;
168			typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
169			typedef typename MathBase::Matrix6xs Matrix6xs;
170
171			template <template <typename Scalar> class Model>
172		✗	ResidualDataFrameTranslationTpl(Model<Scalar>* const model,
173			DataCollectorAbstract* const data)
174		✗	: Base(model, data), fJf(6, model->get_state()->get_nv()) {
175		✗	fJf.setZero();
176			// Check that proper shared data has been passed
177		✗	DataCollectorMultibodyTpl<Scalar>* d =
178		✗	dynamic_cast<DataCollectorMultibodyTpl<Scalar>*>(shared);
179		✗	if (d == NULL) {
180		✗	throw_pretty(
181			"Invalid argument: the shared data should be derived from "
182			"DataCollectorMultibody");
183			}
184
185			// Avoids data casting at runtime
186		✗	pinocchio = d->pinocchio;
187		✗	}
188		✗	virtual ~ResidualDataFrameTranslationTpl() = default;
189
190			pinocchio::DataTpl<Scalar>* pinocchio; //!< Pinocchio data
191			Matrix6xs fJf; //!< Local Jacobian of the frame
192
193			using Base::r;
194			using Base::Ru;
195			using Base::Rx;
196			using Base::shared;
197			};
198
199			} // namespace crocoddyl
200
201			/* --- Details -------------------------------------------------------------- */
202			/* --- Details -------------------------------------------------------------- */
203			/* --- Details -------------------------------------------------------------- */
204			#include "crocoddyl/multibody/residuals/frame-translation.hxx"
205
206			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
207			crocoddyl::ResidualModelFrameTranslationTpl)
208			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
209			crocoddyl::ResidualDataFrameTranslationTpl)
210
211			#endif // CROCODDYL_MULTIBODY_RESIDUALS_FRAME_TRANSLATION_HPP_
212