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_ACTUATIONS_FULL_HPP_
11			#define CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
12
13			#include "crocoddyl/core/actuation-base.hpp"
14			#include "crocoddyl/core/state-base.hpp"
15			#include "crocoddyl/multibody/fwd.hpp"
16
17			namespace crocoddyl {
18
19			/**
20			* @brief Full actuation model
21			*
22			* This actuation model applies input controls for all the `nv` dimensions of
23			* the system.
24			*
25			* Both actuation and Jacobians are computed analytically by `calc` and
26			* `calcDiff`, respectively.
27			*
28			* \sa `ActuationModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
29			*/
30			template <typename _Scalar>
31			class ActuationModelFullTpl : public ActuationModelAbstractTpl<_Scalar> {
32			public:
33			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
34		✗	CROCODDYL_DERIVED_CAST(ActuationModelBase, ActuationModelFullTpl)
35
36			typedef _Scalar Scalar;
37			typedef MathBaseTpl<Scalar> MathBase;
38			typedef ActuationModelAbstractTpl<Scalar> Base;
39			typedef ActuationDataAbstractTpl<Scalar> Data;
40			typedef StateAbstractTpl<Scalar> StateAbstract;
41			typedef typename MathBase::VectorXs VectorXs;
42			typedef typename MathBase::MatrixXs MatrixXs;
43
44			/**
45			* @brief Initialize the full actuation model
46			*
47			* @param[in] state State of the dynamical system
48			*/
49		✗	explicit ActuationModelFullTpl(std::shared_ptr<StateAbstract> state)
50		✗	: Base(state, state->get_nv()) {};
51		✗	virtual ~ActuationModelFullTpl() = default;
52
53			/**
54			* @brief Compute the full actuation
55			*
56			* @param[in] data Full actuation data
57			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
58			* @param[in] u Joint torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
59			*/
60		✗	virtual void calc(const std::shared_ptr<Data>& data,
61			const Eigen::Ref<const VectorXs>& /*x*/,
62			const Eigen::Ref<const VectorXs>& u) override {
63		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
64		✗	throw_pretty(
65			"Invalid argument: " << "u has wrong dimension (it should be " +
66			std::to_string(nu_) + ")");
67			}
68		✗	data->tau = u;
69		✗	};
70
71			/**
72			* @brief Compute the Jacobians of the full actuation model
73			*
74			* @param[in] data Full actuation data
75			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
76			* @param[in] u Joint torque input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
77			*/
78			#ifndef NDEBUG
79		✗	virtual void calcDiff(const std::shared_ptr<Data>& data,
80			const Eigen::Ref<const VectorXs>& /*x*/,
81			const Eigen::Ref<const VectorXs>&) override {
82			#else
83			virtual void calcDiff(const std::shared_ptr<Data>&,
84			const Eigen::Ref<const VectorXs>& /*x*/,
85			const Eigen::Ref<const VectorXs>&) override {
86			#endif
87			// The derivatives has constant values which were set in createData.
88		✗	assert_pretty(data->dtau_dx.isZero(), "dtau_dx has wrong value");
89		✗	assert_pretty(MatrixXs(data->dtau_du)
90			.isApprox(MatrixXs::Identity(state_->get_nv(), nu_)),
91			"dtau_du has wrong value");
92		✗	};
93
94		✗	virtual void commands(const std::shared_ptr<Data>& data,
95			const Eigen::Ref<const VectorXs>&,
96			const Eigen::Ref<const VectorXs>& tau) override {
97		✗	if (static_cast<std::size_t>(tau.size()) != nu_) {
98		✗	throw_pretty(
99			"Invalid argument: " << "tau has wrong dimension (it should be " +
100			std::to_string(nu_) + ")");
101			}
102		✗	data->u = tau;
103		✗	}
104
105			#ifndef NDEBUG
106		✗	virtual void torqueTransform(const std::shared_ptr<Data>& data,
107			const Eigen::Ref<const VectorXs>&,
108			const Eigen::Ref<const VectorXs>&) override {
109			#else
110			virtual void torqueTransform(const std::shared_ptr<Data>&,
111			const Eigen::Ref<const VectorXs>&,
112			const Eigen::Ref<const VectorXs>&) override {
113			#endif
114			// The torque transform has constant values which were set in createData.
115		✗	assert_pretty(MatrixXs(data->Mtau).isApprox(MatrixXs::Identity(nu_, nu_)),
116			"Mtau has wrong value");
117		✗	}
118
119			/**
120			* @brief Create the full actuation data
121			*
122			* @param[in] data shared data (it should be of type DataCollectorContactTpl)
123			* @return the cost data.
124			*/
125		✗	virtual std::shared_ptr<Data> createData() override {
126		✗	std::shared_ptr<Data> data =
127		✗	std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
128		✗	data->dtau_du.diagonal().setOnes();
129		✗	data->Mtau.setIdentity();
130		✗	return data;
131		✗	};
132
133			template <typename NewScalar>
134		✗	ActuationModelFullTpl<NewScalar> cast() const {
135			typedef ActuationModelFullTpl<NewScalar> ReturnType;
136		✗	ReturnType ret(state_->template cast<NewScalar>());
137		✗	return ret;
138			}
139
140			/**
141			* @brief Print relevant information of the joint-effort residual
142			*
143			* @param[out] os Output stream object
144			*/
145		✗	virtual void print(std::ostream& os) const override {
146		✗	os << "ActuationModelFull {nu=" << nu_ << ", nv=" << state_->get_nv()
147		✗	<< "}";
148		✗	}
149
150			protected:
151			using Base::nu_;
152			using Base::state_;
153			};
154
155			} // namespace crocoddyl
156
157			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActuationModelFullTpl)
158
159			#endif // CROCODDYL_MULTIBODY_ACTUATIONS_FULL_HPP_
160