GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/cost.hxx
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 102 0.0%
Branches: 0 144 0.0%
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