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/cost.hpp"
11
12			namespace crocoddyl {
13
14			template <typename Scalar>
15		✗	CostModelNumDiffTpl<Scalar>::CostModelNumDiffTpl(
16			const std::shared_ptr<Base>& model)
17			: Base(model->get_state(), model->get_activation(), 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 CostModelNumDiffTpl<Scalar>::calc(
23			const std::shared_ptr<CostDataAbstract>& data,
24			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
25		✗	Data* d = static_cast<Data*>(data.get());
26		✗	d->data_0->cost = Scalar(0.);
27		✗	model_->calc(d->data_0, x, u);
28		✗	d->cost = d->data_0->cost;
29		✗	d->residual->r = d->data_0->residual->r;
30		✗	}
31
32			template <typename Scalar>
33		✗	void CostModelNumDiffTpl<Scalar>::calc(
34			const std::shared_ptr<CostDataAbstract>& data,
35			const Eigen::Ref<const VectorXs>& x) {
36		✗	Data* d = static_cast<Data*>(data.get());
37		✗	d->data_0->cost = Scalar(0.);
38		✗	model_->calc(d->data_0, x);
39		✗	d->cost = d->data_0->cost;
40		✗	d->residual->r = d->data_0->residual->r;
41		✗	}
42
43			template <typename Scalar>
44		✗	void CostModelNumDiffTpl<Scalar>::calcDiff(
45			const std::shared_ptr<CostDataAbstract>& data,
46			const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
47		✗	Data* d = static_cast<Data*>(data.get());
48
49		✗	const Scalar c0 = d->cost;
50		✗	const VectorXs& r0 = d->residual->r;
51		✗	if (get_with_gauss_approx()) {
52		✗	model_->get_activation()->calc(d->data_0->activation, r0);
53		✗	model_->get_activation()->calcDiff(d->data_0->activation, r0);
54			}
55		✗	d->du.setZero();
56
57		✗	assertStableStateFD(x);
58
59			// Computing the d cost(x,u) / dx
60		✗	model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
61		✗	d->x_norm = d->dx.norm();
62		✗	d->dx.setZero();
63		✗	d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
64		✗	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
65		✗	d->dx(ix) = d->xh_jac;
66		✗	model_->get_state()->integrate(x, d->dx, d->xp);
67			// call the update function on the pinocchio data
68		✗	for (size_t i = 0; i < reevals_.size(); ++i) {
69		✗	reevals_[i](d->xp, u);
70			}
71		✗	model_->calc(d->data_x[ix], d->xp, u);
72		✗	d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
73		✗	if (get_with_gauss_approx()) {
74		✗	d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
75			}
76		✗	d->dx(ix) = Scalar(0.);
77			}
78
79			// Computing the d cost(x,u) / du
80		✗	d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
81		✗	for (std::size_t iu = 0; iu < model_->get_nu(); ++iu) {
82		✗	d->du(iu) = d->uh_jac;
83		✗	d->up = u + d->du;
84			// call the update function
85		✗	for (std::size_t i = 0; i < reevals_.size(); ++i) {
86		✗	reevals_[i](x, d->up);
87			}
88		✗	model_->calc(d->data_u[iu], x, d->up);
89		✗	d->Lu(iu) = (d->data_u[iu]->cost - c0) / d->uh_jac;
90		✗	if (get_with_gauss_approx()) {
91		✗	d->residual->Ru.col(iu) = (d->data_u[iu]->residual->r - r0) / d->uh_jac;
92			}
93		✗	d->du(iu) = Scalar(0.);
94			}
95
96		✗	if (get_with_gauss_approx()) {
97		✗	const MatrixXs& Arr = d->data_0->activation->Arr;
98		✗	d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
99		✗	d->Lxu = d->residual->Rx.transpose() * Arr * d->residual->Ru;
100		✗	d->Luu = d->residual->Ru.transpose() * Arr * d->residual->Ru;
101		✗	} else {
102		✗	d->Lxx.fill(Scalar(0.));
103		✗	d->Lxu.fill(Scalar(0.));
104		✗	d->Luu.fill(Scalar(0.));
105			}
106		✗	}
107
108			template <typename Scalar>
109		✗	void CostModelNumDiffTpl<Scalar>::calcDiff(
110			const std::shared_ptr<CostDataAbstract>& data,
111			const Eigen::Ref<const VectorXs>& x) {
112		✗	Data* d = static_cast<Data*>(data.get());
113
114		✗	const Scalar c0 = d->cost;
115		✗	const VectorXs& r0 = d->residual->r;
116		✗	if (get_with_gauss_approx()) {
117		✗	model_->get_activation()->calc(d->data_0->activation, r0);
118		✗	model_->get_activation()->calcDiff(d->data_0->activation, r0);
119			}
120		✗	d->dx.setZero();
121
122		✗	assertStableStateFD(x);
123
124			// Computing the d cost(x,u) / dx
125		✗	d->xh_jac = e_jac_ * std::max(Scalar(1.), x.norm());
126		✗	for (std::size_t ix = 0; ix < state_->get_ndx(); ++ix) {
127		✗	d->dx(ix) = d->xh_jac;
128		✗	model_->get_state()->integrate(x, d->dx, d->xp);
129			// call the update function on the pinocchio data
130		✗	for (size_t i = 0; i < reevals_.size(); ++i) {
131		✗	reevals_[i](d->xp, unone_);
132			}
133		✗	model_->calc(d->data_x[ix], d->xp);
134		✗	d->Lx(ix) = (d->data_x[ix]->cost - c0) / d->xh_jac;
135		✗	if (get_with_gauss_approx()) {
136		✗	d->residual->Rx.col(ix) = (d->data_x[ix]->residual->r - r0) / d->xh_jac;
137			}
138		✗	d->dx(ix) = Scalar(0.);
139			}
140
141		✗	if (get_with_gauss_approx()) {
142		✗	const MatrixXs& Arr = d->data_0->activation->Arr;
143		✗	d->Lxx = d->residual->Rx.transpose() * Arr * d->residual->Rx;
144		✗	} else {
145		✗	d->Lxx.fill(Scalar(0.));
146			}
147		✗	}
148
149			template <typename Scalar>
150			std::shared_ptr<CostDataAbstractTpl<Scalar> >
151		✗	CostModelNumDiffTpl<Scalar>::createData(DataCollectorAbstract* const data) {
152		✗	return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this,
153		✗	data);
154			}
155
156			template <typename Scalar>
157			template <typename NewScalar>
158		✗	CostModelNumDiffTpl<NewScalar> CostModelNumDiffTpl<Scalar>::cast() const {
159			typedef CostModelNumDiffTpl<NewScalar> ReturnType;
160		✗	ReturnType res(model_->template cast<NewScalar>());
161		✗	return res;
162			}
163
164			template <typename Scalar>
165			const std::shared_ptr<CostModelAbstractTpl<Scalar> >&
166		✗	CostModelNumDiffTpl<Scalar>::get_model() const {
167		✗	return model_;
168			}
169
170			template <typename Scalar>
171		✗	const Scalar CostModelNumDiffTpl<Scalar>::get_disturbance() const {
172		✗	return e_jac_;
173			}
174
175			template <typename Scalar>
176		✗	void CostModelNumDiffTpl<Scalar>::set_disturbance(const Scalar disturbance) {
177		✗	if (disturbance < Scalar(0.)) {
178		✗	throw_pretty("Invalid argument: " << "Disturbance constant is positive");
179			}
180		✗	e_jac_ = disturbance;
181		✗	}
182
183			template <typename Scalar>
184		✗	bool CostModelNumDiffTpl<Scalar>::get_with_gauss_approx() {
185		✗	return activation_->get_nr() > 0;
186			}
187
188			template <typename Scalar>
189		✗	void CostModelNumDiffTpl<Scalar>::set_reevals(
190			const std::vector<ReevaluationFunction>& reevals) {
191		✗	reevals_ = reevals;
192		✗	}
193
194			template <typename Scalar>
195		✗	void CostModelNumDiffTpl<Scalar>::assertStableStateFD(
196			const Eigen::Ref<const VectorXs>& /*x*/) {
197			// do nothing in the general case
198		✗	}
199
200			} // namespace crocoddyl
201