GCC Code Coverage Report

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