GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2014-2025, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#ifndef CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
10			#define CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
11
12			#include "crocoddyl/core/actuation-base.hpp"
13			#include "crocoddyl/multibody/states/multibody.hpp"
14
15			namespace crocoddyl {
16
17			enum ThrusterType { CW = 0, CCW };
18
19			template <typename _Scalar>
20			struct ThrusterTpl {
21			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22
23			typedef _Scalar Scalar;
24			typedef pinocchio::SE3Tpl<Scalar> SE3;
25			typedef ThrusterTpl<Scalar> Thruster;
26
27			/**
28			* @brief Initialize the thruster in a give pose from the root joint.
29			*
30			* @param pose[in] Pose from root joint
31			* @param ctorque[in] Coefficient of generated torque per thrust
32			* @param type[in] Type of thruster (clockwise or counterclockwise,
33			* default clockwise)
34			* @param[in] min_thrust[in] Minimum thrust (default 0.)
35			* @param[in] max_thrust[in] Maximum thrust (default inf number))
36			*/
37		✗	ThrusterTpl(const SE3& pose, const Scalar ctorque,
38			const ThrusterType type = CW,
39			const Scalar min_thrust = Scalar(0.),
40			const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
41		✗	: pose(pose),
42		✗	ctorque(ctorque),
43		✗	type(type),
44		✗	min_thrust(min_thrust),
45		✗	max_thrust(max_thrust) {}
46
47			/**
48			* @brief Initialize the thruster in a pose in the origin of the root joint.
49			*
50			* @param pose[in] Pose from root joint
51			* @param ctorque[in] Coefficient of generated torque per thrust
52			* @param type[in] Type of thruster (clockwise or counterclockwise,
53			* default clockwise)
54			* @param[in] min_thrust[in] Minimum thrust (default 0.)
55			* @param[in] max_thrust[in] Maximum thrust (default inf number))
56			*/
57		✗	ThrusterTpl(const Scalar ctorque, const ThrusterType type = CW,
58			const Scalar min_thrust = Scalar(0.),
59			const Scalar max_thrust = std::numeric_limits<Scalar>::infinity())
60		✗	: pose(SE3::Identity()),
61		✗	ctorque(ctorque),
62		✗	type(type),
63		✗	min_thrust(min_thrust),
64		✗	max_thrust(max_thrust) {}
65		✗	ThrusterTpl(const ThrusterTpl<Scalar>& clone)
66		✗	: pose(clone.pose),
67		✗	ctorque(clone.ctorque),
68		✗	type(clone.type),
69		✗	min_thrust(clone.min_thrust),
70		✗	max_thrust(clone.max_thrust) {}
71
72			template <typename NewScalar>
73		✗	ThrusterTpl<NewScalar> cast() const {
74			typedef ThrusterTpl<NewScalar> ReturnType;
75		✗	ReturnType ret(
76		✗	pose.template cast<NewScalar>(), scalar_cast<NewScalar>(ctorque), type,
77		✗	scalar_cast<NewScalar>(min_thrust), scalar_cast<NewScalar>(max_thrust));
78		✗	return ret;
79			}
80
81		✗	ThrusterTpl& operator=(const ThrusterTpl<Scalar>& other) {
82		✗	if (this != &other) {
83		✗	pose = other.pose;
84		✗	ctorque = other.ctorque;
85		✗	type = other.type;
86		✗	min_thrust = other.min_thrust;
87		✗	max_thrust = other.max_thrust;
88			}
89		✗	return *this;
90			}
91
92			template <typename OtherScalar>
93		✗	bool operator==(const ThrusterTpl<OtherScalar>& other) const {
94		✗	return (pose == other.pose && ctorque == other.ctorque &&
95		✗	type == other.type && min_thrust == other.min_thrust &&
96		✗	max_thrust == other.max_thrust);
97			}
98
99		✗	friend std::ostream& operator<<(std::ostream& os,
100			const ThrusterTpl<Scalar>& X) {
101		✗	os << " pose:" << std::endl
102		✗	<< X.pose << " ctorque: " << X.ctorque << std::endl
103		✗	<< " type: " << X.type << std::endl
104		✗	<< "min_thrust: " << X.min_thrust << std::endl
105		✗	<< "max_thrust: " << X.max_thrust << std::endl;
106		✗	return os;
107			}
108
109			SE3 pose; //!< Thruster pose
110			Scalar ctorque; //!< Coefficient of generated torque per thrust
111			ThrusterType type; //!< Type of thruster (CW and CCW for clockwise and
112			//!< counterclockwise, respectively)
113			Scalar min_thrust; //!< Minimum thrust
114			Scalar max_thrust; //!< Minimum thrust
115			};
116
117			/**
118			* @brief Actuation models for floating base systems actuated with thrusters
119			*
120			* This actuation model models floating base robots equipped with thrusters,
121			* e.g., multicopters or marine robots equipped with manipulators. It control
122			* inputs are the thrusters' thrust (i.e., forces) and joint torques.
123			*
124			* Both actuation and Jacobians are computed analytically by `calc` and
125			* `calcDiff`, respectively.
126			*
127			* We assume the robot velocity to zero for easily related square thruster
128			* velocities with thrust and torque generated. This approach is similarly
129			* implemented in M. Geisert and N. Mansard, "Trajectory generation for
130			* quadrotor based systems using numerical optimal control", (ICRA). See Section
131			* III.C.
132			*
133			* \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
134			*/
135			template <typename _Scalar>
136			class ActuationModelFloatingBaseThrustersTpl
137			: public ActuationModelAbstractTpl<_Scalar> {
138			public:
139			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
140		✗	CROCODDYL_DERIVED_CAST(ActuationModelBase,
141			ActuationModelFloatingBaseThrustersTpl)
142
143			typedef _Scalar Scalar;
144			typedef MathBaseTpl<Scalar> MathBase;
145			typedef ActuationModelAbstractTpl<Scalar> Base;
146			typedef ActuationDataAbstractTpl<Scalar> Data;
147			typedef StateMultibodyTpl<Scalar> StateMultibody;
148			typedef ThrusterTpl<Scalar> Thruster;
149			typedef typename MathBase::Vector3s Vector3s;
150			typedef typename MathBase::VectorXs VectorXs;
151			typedef typename MathBase::MatrixXs MatrixXs;
152
153			/**
154			* @brief Initialize the floating base actuation model equipped with
155			* thrusters
156			*
157			* @param[in] state State of the dynamical system
158			* @param[in] thrusters Vector of thrusters
159			*/
160		✗	ActuationModelFloatingBaseThrustersTpl(std::shared_ptr<StateMultibody> state,
161			const std::vector<Thruster>& thrusters)
162			: Base(state,
163		✗	state->get_nv() -
164		✗	state->get_pinocchio()
165		✗	->joints[(
166		✗	state->get_pinocchio()->existJointName("root_joint")
167		✗	? state->get_pinocchio()->getJointId("root_joint")
168			: 0)]
169		✗	.nv() +
170		✗	thrusters.size()),
171		✗	thrusters_(thrusters),
172		✗	n_thrusters_(thrusters.size()),
173		✗	W_thrust_(state_->get_nv(), nu_),
174		✗	update_data_(true) {
175		✗	if (!state->get_pinocchio()->existJointName("root_joint")) {
176		✗	throw_pretty(
177			"Invalid argument: "
178			<< "the first joint has to be a root one (e.g., free-flyer joint)");
179			}
180			// Update the joint actuation part
181		✗	W_thrust_.setZero();
182		✗	if (nu_ > n_thrusters_) {
183		✗	W_thrust_.bottomRightCorner(nu_ - n_thrusters_, nu_ - n_thrusters_)
184			.diagonal()
185		✗	.setOnes();
186			}
187			// Update the floating base actuation part
188		✗	set_thrusters(thrusters_);
189		✗	}
190		✗	virtual ~ActuationModelFloatingBaseThrustersTpl() = default;
191
192			/**
193			* @brief Compute the actuation signal and actuation set from its thrust
194			* and joint torque inputs \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
195			*
196			* @param[in] data Floating base thrusters actuation data
197			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
198			* @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
199			*/
200		✗	virtual void calc(const std::shared_ptr<Data>& data,
201			const Eigen::Ref<const VectorXs>&,
202			const Eigen::Ref<const VectorXs>& u) override {
203		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
204		✗	throw_pretty(
205			"Invalid argument: " << "u has wrong dimension (it should be " +
206			std::to_string(nu_) + ")");
207			}
208		✗	if (update_data_) {
209		✗	updateData(data);
210			}
211		✗	data->tau.noalias() = data->dtau_du * u;
212		✗	}
213
214			/**
215			* @brief Compute the Jacobians of the floating base thruster actuation
216			* function
217			*
218			* @param[in] data Floating base thrusters actuation data
219			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
220			* @param[in] u Joint-torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
221			*/
222			#ifndef NDEBUG
223		✗	virtual void calcDiff(const std::shared_ptr<Data>& data,
224			const Eigen::Ref<const VectorXs>&,
225			const Eigen::Ref<const VectorXs>&) override {
226			#else
227			virtual void calcDiff(const std::shared_ptr<Data>&,
228			const Eigen::Ref<const VectorXs>&,
229			const Eigen::Ref<const VectorXs>&) override {
230			#endif
231			// The derivatives has constant values which were set in createData.
232		✗	assert_pretty(MatrixXs(data->dtau_du).isApprox(W_thrust_),
233			"dtau_du has wrong value");
234		✗	}
235
236		✗	virtual void commands(const std::shared_ptr<Data>& data,
237			const Eigen::Ref<const VectorXs>&,
238			const Eigen::Ref<const VectorXs>& tau) override {
239		✗	data->u.noalias() = data->Mtau * tau;
240		✗	}
241
242			#ifndef NDEBUG
243		✗	virtual void torqueTransform(const std::shared_ptr<Data>& data,
244			const Eigen::Ref<const VectorXs>&,
245			const Eigen::Ref<const VectorXs>&) override {
246			#else
247			virtual void torqueTransform(const std::shared_ptr<Data>&,
248			const Eigen::Ref<const VectorXs>&,
249			const Eigen::Ref<const VectorXs>&) override {
250			#endif
251			// The torque transform has constant values which were set in createData.
252		✗	assert_pretty(MatrixXs(data->Mtau).isApprox(Mtau_), "Mtau has wrong value");
253		✗	}
254
255			/**
256			* @brief Create the floating base thruster actuation data
257			*
258			* @return the actuation data
259			*/
260		✗	virtual std::shared_ptr<Data> createData() override {
261		✗	std::shared_ptr<Data> data =
262		✗	std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
263		✗	updateData(data);
264		✗	return data;
265		✗	}
266
267			template <typename NewScalar>
268		✗	ActuationModelFloatingBaseThrustersTpl<NewScalar> cast() const {
269			typedef ActuationModelFloatingBaseThrustersTpl<NewScalar> ReturnType;
270			typedef StateMultibodyTpl<NewScalar> StateType;
271			typedef ThrusterTpl<NewScalar> ThrusterType;
272		✗	std::vector<ThrusterType> thrusters = vector_cast<NewScalar>(thrusters_);
273		✗	ReturnType ret(
274		✗	std::static_pointer_cast<StateType>(state_->template cast<NewScalar>()),
275			thrusters);
276		✗	return ret;
277		✗	}
278
279			/**
280			* @brief Return the vector of thrusters
281			*/
282		✗	const std::vector<Thruster>& get_thrusters() const { return thrusters_; }
283
284			/**
285			* @brief Return the number of thrusters
286			*/
287		✗	std::size_t get_nthrusters() const { return n_thrusters_; }
288
289			/**
290			* @brief Modify the vector of thrusters
291			*
292			* Since we don't want to allocate data, we request to pass the same
293			* number of thrusters.
294			*
295			* @param[in] thrusters Vector of thrusters
296			*/
297		✗	void set_thrusters(const std::vector<Thruster>& thrusters) {
298		✗	if (static_cast<std::size_t>(thrusters.size()) != n_thrusters_) {
299		✗	throw_pretty("Invalid argument: "
300			<< "the number of thrusters is wrong (it should be " +
301			std::to_string(n_thrusters_) + ")");
302			}
303		✗	thrusters_ = thrusters;
304			// Update the mapping matrix from thrusters thrust to body force/moments
305		✗	for (std::size_t i = 0; i < n_thrusters_; ++i) {
306		✗	const Thruster& p = thrusters_[i];
307		✗	const Vector3s& f_z = p.pose.rotation() * Vector3s::UnitZ();
308		✗	W_thrust_.template topRows<3>().col(i) += f_z;
309		✗	W_thrust_.template middleRows<3>(3).col(i).noalias() +=
310		✗	p.pose.translation().cross(f_z);
311		✗	switch (p.type) {
312		✗	case CW:
313		✗	W_thrust_.template middleRows<3>(3).col(i) += p.ctorque * f_z;
314		✗	break;
315		✗	case CCW:
316		✗	W_thrust_.template middleRows<3>(3).col(i) -= p.ctorque * f_z;
317		✗	break;
318			}
319			}
320			// Compute the torque transform matrix from generalized torques to joint
321			// torque inputs
322		✗	Mtau_ = pseudoInverse(W_thrust_);
323		✗	S_.noalias() = W_thrust_ * Mtau_;
324		✗	update_data_ = true;
325		✗	}
326
327		✗	const MatrixXs& get_Wthrust() const { return W_thrust_; }
328
329		✗	const MatrixXs& get_S() const { return S_; }
330
331		✗	void print(std::ostream& os) const override {
332		✗	os << "ActuationModelFloatingBaseThrusters {nu=" << nu_
333		✗	<< ", nthrusters=" << n_thrusters_ << ", thrusters=" << std::endl;
334		✗	for (std::size_t i = 0; i < n_thrusters_; ++i) {
335		✗	os << std::to_string(i) << ": " << thrusters_[i];
336			}
337		✗	os << "}";
338		✗	}
339
340			protected:
341			std::vector<Thruster> thrusters_; //!< Vector of thrusters
342			std::size_t n_thrusters_; //!< Number of thrusters
343			MatrixXs W_thrust_; //!< Matrix from thrusters thrusts to body wrench
344			MatrixXs Mtau_; //!< Constaint torque transform from generalized torques to
345			//!< joint torque inputs
346			MatrixXs S_; //!< Selection matrix for under-actuation part
347
348			bool update_data_;
349			using Base::nu_;
350			using Base::state_;
351
352			private:
353		✗	void updateData(const std::shared_ptr<Data>& data) {
354		✗	data->dtau_du = W_thrust_;
355		✗	data->Mtau = Mtau_;
356		✗	const std::size_t nv = state_->get_nv();
357		✗	for (std::size_t k = 0; k < nv; ++k) {
358		✗	data->tau_set[k] = if_static(S_(k, k));
359			}
360		✗	update_data_ = false;
361		✗	}
362
363			// Use for floating-point types
364			template <typename Scalar>
365			typename std::enable_if<std::is_floating_point<Scalar>::value, bool>::type
366		✗	if_static(const Scalar& condition) {
367		✗	return (fabs(condition) < std::numeric_limits<Scalar>::epsilon()) ? false
368		✗	: true;
369			}
370
371			#ifdef CROCODDYL_WITH_CODEGEN
372			// Use for CppAD types
373			template <typename Scalar>
374			typename std::enable_if<!std::is_floating_point<Scalar>::value, bool>::type
375			if_static(const Scalar& condition) {
376			return CppAD::Value(CppAD::fabs(condition)) >=
377			CppAD::numeric_limits<Scalar>::epsilon();
378			}
379			#endif
380			};
381
382			} // namespace crocoddyl
383
384			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ThrusterTpl)
385			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
386			crocoddyl::ActuationModelFloatingBaseThrustersTpl)
387
388			#endif // CROCODDYL_MULTIBODY_ACTUATIONS_PROPELLERS_HPP_
389