GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, University of Edinburgh, LAAS-CNRS,
5			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#include "crocoddyl/core/numdiff/actuation.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		✗	ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(
16			std::shared_ptr<Base> model)
17			: Base(model->get_state(), model->get_nu()),
18		✗	model_(model),
19		✗	e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
20
21			template <typename Scalar>
22		✗	void ActuationModelNumDiffTpl<Scalar>::calc(
23			const std::shared_ptr<ActuationDataAbstract>& data,
24			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
25		✗	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
26		✗	throw_pretty("Invalid argument: "
27			<< "x has wrong dimension (it should be " +
28			std::to_string(model_->get_state()->get_nx()) + ")");
29			}
30		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
31		✗	throw_pretty(
32			"Invalid argument: " << "u has wrong dimension (it should be " +
33			std::to_string(nu_) + ")");
34			}
35		✗	Data* d = static_cast<Data*>(data.get());
36		✗	model_->calc(d->data_0, x, u);
37		✗	data->tau = d->data_0->tau;
38		✗	}
39
40			template <typename Scalar>
41		✗	void ActuationModelNumDiffTpl<Scalar>::calc(
42			const std::shared_ptr<ActuationDataAbstract>& data,
43			const Eigen::Ref<const VectorXs>& x) {
44		✗	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
45		✗	throw_pretty("Invalid argument: "
46			<< "x has wrong dimension (it should be " +
47			std::to_string(model_->get_state()->get_nx()) + ")");
48			}
49		✗	Data* d = static_cast<Data*>(data.get());
50		✗	model_->calc(d->data_0, x);
51		✗	data->tau = d->data_0->tau;
52		✗	}
53
54			template <typename Scalar>
55		✗	void ActuationModelNumDiffTpl<Scalar>::calcDiff(
56			const std::shared_ptr<ActuationDataAbstract>& data,
57			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
58		✗	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
59		✗	throw_pretty("Invalid argument: "
60			<< "x has wrong dimension (it should be " +
61			std::to_string(model_->get_state()->get_nx()) + ")");
62			}
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* d = static_cast<Data*>(data.get());
69		✗	const VectorXs& tau0 = d->data_0->tau;
70		✗	d->du.setZero();
71
72			// Computing the d actuation(x,u) / dx
73		✗	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
74		✗	d->x_norm = d->dx.norm();
75		✗	d->dx.setZero();
76		✗	d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
77		✗	for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
78		✗	d->dx(ix) = d->xh_jac;
79		✗	model_->get_state()->integrate(x, d->dx, d->xp);
80		✗	model_->calc(d->data_x[ix], d->xp, u);
81		✗	d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
82		✗	d->dx(ix) = Scalar(0.);
83			}
84
85			// Computing the d actuation(x,u) / du
86		✗	d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
87		✗	for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
88		✗	d->du(iu) = d->uh_jac;
89		✗	model_->calc(d->data_u[iu], x, u + d->du);
90		✗	d->dtau_du.col(iu) = (d->data_u[iu]->tau - tau0) / d->uh_jac;
91		✗	d->du(iu) = Scalar(0.);
92			}
93		✗	}
94
95			template <typename Scalar>
96		✗	void ActuationModelNumDiffTpl<Scalar>::calcDiff(
97			const std::shared_ptr<ActuationDataAbstract>& data,
98			const Eigen::Ref<const VectorXs>& x) {
99		✗	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
100		✗	throw_pretty("Invalid argument: "
101			<< "x has wrong dimension (it should be " +
102			std::to_string(model_->get_state()->get_nx()) + ")");
103			}
104		✗	Data* d = static_cast<Data*>(data.get());
105		✗	const VectorXs& tau0 = d->data_0->tau;
106		✗	d->dx.setZero();
107
108			// Computing the d actuation(x,u) / dx
109		✗	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
110		✗	d->x_norm = d->dx.norm();
111		✗	d->dx.setZero();
112		✗	d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
113		✗	for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
114		✗	d->dx(ix) = d->xh_jac;
115		✗	model_->get_state()->integrate(x, d->dx, d->xp);
116		✗	model_->calc(d->data_x[ix], d->xp);
117		✗	d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
118		✗	d->dx(ix) = Scalar(0.);
119			}
120		✗	}
121
122			template <typename Scalar>
123		✗	void ActuationModelNumDiffTpl<Scalar>::commands(
124			const std::shared_ptr<ActuationDataAbstract>& data,
125			const Eigen::Ref<const VectorXs>& x,
126			const Eigen::Ref<const VectorXs>& tau) {
127		✗	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
128		✗	throw_pretty("Invalid argument: "
129			<< "x has wrong dimension (it should be " +
130			std::to_string(model_->get_state()->get_nx()) + ")");
131			}
132		✗	if (static_cast<std::size_t>(tau.size()) != model_->get_state()->get_nv()) {
133		✗	throw_pretty("Invalid argument: "
134			<< "tau has wrong dimension (it should be " +
135			std::to_string(model_->get_state()->get_nv()) + ")");
136			}
137		✗	Data* d = static_cast<Data*>(data.get());
138		✗	model_->commands(d->data_0, x, tau);
139		✗	data->u = d->data_0->u;
140		✗	}
141
142			template <typename Scalar>
143		✗	void ActuationModelNumDiffTpl<Scalar>::torqueTransform(
144			const std::shared_ptr<ActuationDataAbstract>& data,
145			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
146		✗	if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
147		✗	throw_pretty("Invalid argument: "
148			<< "x has wrong dimension (it should be " +
149			std::to_string(model_->get_state()->get_nx()) + ")");
150			}
151		✗	if (static_cast<std::size_t>(u.size()) != nu_) {
152		✗	throw_pretty(
153			"Invalid argument: " << "u has wrong dimension (it should be " +
154			std::to_string(nu_) + ")");
155			}
156		✗	Data* d = static_cast<Data*>(data.get());
157		✗	model_->torqueTransform(d->data_0, x, u);
158		✗	d->Mtau = d->data_0->Mtau;
159		✗	}
160
161			template <typename Scalar>
162			std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
163		✗	ActuationModelNumDiffTpl<Scalar>::createData() {
164		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
165			}
166
167			template <typename Scalar>
168			template <typename NewScalar>
169		✗	ActuationModelNumDiffTpl<NewScalar> ActuationModelNumDiffTpl<Scalar>::cast()
170			const {
171			typedef ActuationModelNumDiffTpl<NewScalar> ReturnType;
172		✗	ReturnType res(model_->template cast<NewScalar>());
173		✗	return res;
174			}
175
176			template <typename Scalar>
177			const std::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
178		✗	ActuationModelNumDiffTpl<Scalar>::get_model() const {
179		✗	return model_;
180			}
181
182			template <typename Scalar>
183		✗	const Scalar ActuationModelNumDiffTpl<Scalar>::get_disturbance() const {
184		✗	return e_jac_;
185			}
186
187			template <typename Scalar>
188			void ActuationModelNumDiffTpl<Scalar>::set_disturbance(
189			const Scalar disturbance) {
190			if (disturbance < Scalar(0.)) {
191			throw_pretty("Invalid argument: " << "Disturbance constant is positive");
192			}
193			e_jac_ = disturbance;
194			}
195
196			} // namespace crocoddyl
197