GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2021-2025, University of Edinburgh, Heriot-Watt University
5			// Copyright note valid unless otherwise stated in individual files.
6			// All rights reserved.
7			///////////////////////////////////////////////////////////////////////////////
8
9			#include "crocoddyl/core/numdiff/residual.hpp"
10
11			namespace crocoddyl {
12
13			template <typename Scalar>
14		✗	ResidualModelNumDiffTpl<Scalar>::ResidualModelNumDiffTpl(
15			const std::shared_ptr<Base>& model)
16			: Base(model->get_state(), model->get_nr(), model->get_nu()),
17		✗	model_(model),
18		✗	e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
19
20			template <typename Scalar>
21		✗	void ResidualModelNumDiffTpl<Scalar>::calc(
22			const std::shared_ptr<ResidualDataAbstract>& data,
23			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
24		✗	Data* d = static_cast<Data*>(data.get());
25		✗	model_->calc(d->data_0, x, u);
26		✗	d->r = d->data_0->r;
27		✗	}
28
29			template <typename Scalar>
30		✗	void ResidualModelNumDiffTpl<Scalar>::calc(
31			const std::shared_ptr<ResidualDataAbstract>& data,
32			const Eigen::Ref<const VectorXs>& x) {
33		✗	Data* d = static_cast<Data*>(data.get());
34		✗	model_->calc(d->data_0, x);
35		✗	d->r = d->data_0->r;
36		✗	}
37
38			template <typename Scalar>
39		✗	void ResidualModelNumDiffTpl<Scalar>::calcDiff(
40			const std::shared_ptr<ResidualDataAbstract>& data,
41			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
42		✗	Data* d = static_cast<Data*>(data.get());
43
44		✗	const VectorXs& r0 = d->r;
45		✗	d->dx.setZero();
46		✗	d->du.setZero();
47
48		✗	assertStableStateFD(x);
49
50			// Computing the d residual(x,u) / dx
51		✗	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
52		✗	d->x_norm = d->dx.norm();
53		✗	d->dx.setZero();
54		✗	d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
55		✗	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
56		✗	d->dx(ix) = d->xh_jac;
57		✗	model_->get_state()->integrate(x, d->dx, d->xp);
58			// call the update function
59		✗	for (size_t i = 0; i < reevals_.size(); ++i) {
60		✗	reevals_[i](d->xp, u);
61			}
62		✗	model_->calc(d->data_x[ix], d->xp, u);
63		✗	d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
64		✗	d->dx(ix) = Scalar(0.);
65			}
66
67			// Computing the d residual(x,u) / du
68		✗	d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
69		✗	for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
70		✗	d->du(iu) = d->uh_jac;
71		✗	d->up = u + d->du;
72			// call the update function
73		✗	for (std::size_t i = 0; i < reevals_.size(); ++i) {
74		✗	reevals_[i](x, d->up);
75			}
76		✗	model_->calc(d->data_u[iu], x, d->up);
77		✗	d->Ru.col(iu) = (d->data_u[iu]->r - r0) / d->uh_jac;
78		✗	d->du(iu) = Scalar(0.);
79			}
80		✗	}
81
82			template <typename Scalar>
83		✗	void ResidualModelNumDiffTpl<Scalar>::calcDiff(
84			const std::shared_ptr<ResidualDataAbstract>& data,
85			const Eigen::Ref<const VectorXs>& x) {
86		✗	Data* d = static_cast<Data*>(data.get());
87
88		✗	const VectorXs& r0 = d->r;
89		✗	assertStableStateFD(x);
90
91			// Computing the d residual(x,u) / dx
92		✗	d->dx.setZero();
93		✗	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
94		✗	d->dx(ix) = d->xh_jac;
95		✗	model_->get_state()->integrate(x, d->dx, d->xp);
96			// call the update function
97		✗	for (size_t i = 0; i < reevals_.size(); ++i) {
98		✗	reevals_[i](d->xp, unone_);
99			}
100		✗	model_->calc(d->data_x[ix], d->xp);
101		✗	d->Rx.col(ix) = (d->data_x[ix]->r - r0) / d->xh_jac;
102		✗	d->dx(ix) = Scalar(0.);
103			}
104		✗	}
105
106			template <typename Scalar>
107			std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
108		✗	ResidualModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
109		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
110		✗	data);
111			}
112
113			template <typename Scalar>
114			template <typename NewScalar>
115		✗	ResidualModelNumDiffTpl<NewScalar> ResidualModelNumDiffTpl<Scalar>::cast()
116			const {
117			typedef ResidualModelNumDiffTpl<NewScalar> ReturnType;
118		✗	ReturnType res(model_->template cast<NewScalar>());
119		✗	return res;
120			}
121
122			template <typename Scalar>
123			const std::shared_ptr<ResidualModelAbstractTpl<Scalar> >&
124		✗	ResidualModelNumDiffTpl<Scalar>::get_model() const {
125		✗	return model_;
126			}
127
128			template <typename Scalar>
129		✗	const Scalar ResidualModelNumDiffTpl<Scalar>::get_disturbance() const {
130		✗	return e_jac_;
131			}
132
133			template <typename Scalar>
134			void ResidualModelNumDiffTpl<Scalar>::set_disturbance(
135			const Scalar disturbance) {
136			if (disturbance < Scalar(0.)) {
137			throw_pretty("Invalid argument: " << "Disturbance constant is positive");
138			}
139			e_jac_ = disturbance;
140			}
141
142			template <typename Scalar>
143		✗	void ResidualModelNumDiffTpl<Scalar>::set_reevals(
144			const std::vector<ReevaluationFunction>& reevals) {
145		✗	reevals_ = reevals;
146		✗	}
147
148			template <typename Scalar>
149		✗	void ResidualModelNumDiffTpl<Scalar>::assertStableStateFD(
150			const Eigen::Ref<const VectorXs>& /*x*/) {
151			// do nothing in the general case
152		✗	}
153
154			} // namespace crocoddyl
155