GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/actuation.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 69 82 84.1%
Branches: 43 270 15.9%
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 144 ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(
16 std::shared_ptr<Base> model)
17 : Base(model->get_state(), model->get_nu()),
18 144 model_(model),
19
1/2
✓ Branch 6 taken 72 times.
✗ Branch 7 not taken.
 144 e_jac_(sqrt(Scalar(2.0) * std::numeric_limits<Scalar>::epsilon())) {}
20
21 template <typename Scalar>
22 72 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
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 36 times.
 72 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 72 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 72 Data* d = static_cast<Data*>(data.get());
36 72 model_->calc(d->data_0, x, u);
37 72 data->tau = d->data_0->tau;
38 }
39
40 template <typename Scalar>
41 36 void ActuationModelNumDiffTpl<Scalar>::calc(
42 const std::shared_ptr<ActuationDataAbstract>& data,
43 const Eigen::Ref<const VectorXs>& x) {
44
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 36 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 36 Data* d = static_cast<Data*>(data.get());
50 36 model_->calc(d->data_0, x);
51 36 data->tau = d->data_0->tau;
52 }
53
54 template <typename Scalar>
55 72 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
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 36 times.
 72 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 72 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 72 Data* d = static_cast<Data*>(data.get());
69 72 const VectorXs& tau0 = d->data_0->tau;
70 72 d->du.setZero();
71
72 // Computing the d actuation(x,u) / dx
73
4/8
✓ Branch 6 taken 36 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 36 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 36 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 36 times.
✗ Branch 17 not taken.
 72 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
74 72 d->x_norm = d->dx.norm();
75 72 d->dx.setZero();
76 72 d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
77
2/2
✓ Branch 4 taken 1432 times.
✓ Branch 5 taken 36 times.
 2936 for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
78 2864 d->dx(ix) = d->xh_jac;
79
2/4
✓ Branch 5 taken 1432 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1432 times.
✗ Branch 9 not taken.
 2864 model_->get_state()->integrate(x, d->dx, d->xp);
80
1/2
✓ Branch 4 taken 1432 times.
✗ Branch 5 not taken.
 2864 model_->calc(d->data_x[ix], d->xp, u);
81
3/6
✓ Branch 4 taken 1432 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1432 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1432 times.
✗ Branch 11 not taken.
 2864 d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
82 2864 d->dx(ix) = Scalar(0.);
83 }
84
85 // Computing the d actuation(x,u) / du
86 72 d->uh_jac = e_jac_ * std::max(Scalar(1.), u.norm());
87
2/2
✓ Branch 2 taken 672 times.
✓ Branch 3 taken 36 times.
 1416 for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
88 1344 d->du(iu) = d->uh_jac;
89
2/4
✓ Branch 3 taken 672 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 672 times.
✗ Branch 8 not taken.
 1344 model_->calc(d->data_u[iu], x, u + d->du);
90
3/6
✓ Branch 4 taken 672 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 672 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 672 times.
✗ Branch 11 not taken.
 1344 d->dtau_du.col(iu) = (d->data_u[iu]->tau - tau0) / d->uh_jac;
91 1344 d->du(iu) = Scalar(0.);
92 }
93 }
94
95 template <typename Scalar>
96 36 void ActuationModelNumDiffTpl<Scalar>::calcDiff(
97 const std::shared_ptr<ActuationDataAbstract>& data,
98 const Eigen::Ref<const VectorXs>& x) {
99
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 36 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 36 Data* d = static_cast<Data*>(data.get());
105 36 const VectorXs& tau0 = d->data_0->tau;
106 36 d->dx.setZero();
107
108 // Computing the d actuation(x,u) / dx
109
4/8
✓ Branch 6 taken 18 times.
✗ Branch 7 not taken.
✓ Branch 10 taken 18 times.
✗ Branch 11 not taken.
✓ Branch 13 taken 18 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 18 times.
✗ Branch 17 not taken.
 36 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
110 36 d->x_norm = d->dx.norm();
111 36 d->dx.setZero();
112 36 d->xh_jac = e_jac_ * std::max(Scalar(1.), d->x_norm);
113
2/2
✓ Branch 4 taken 716 times.
✓ Branch 5 taken 18 times.
 1468 for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
114 1432 d->dx(ix) = d->xh_jac;
115
2/4
✓ Branch 5 taken 716 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 716 times.
✗ Branch 9 not taken.
 1432 model_->get_state()->integrate(x, d->dx, d->xp);
116
1/2
✓ Branch 4 taken 716 times.
✗ Branch 5 not taken.
 1432 model_->calc(d->data_x[ix], d->xp);
117
3/6
✓ Branch 4 taken 716 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 716 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 716 times.
✗ Branch 11 not taken.
 1432 d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
118 1432 d->dx(ix) = Scalar(0.);
119 }
120 }
121
122 template <typename Scalar>
123 36 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
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 36 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
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 36 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 36 Data* d = static_cast<Data*>(data.get());
138 36 model_->commands(d->data_0, x, tau);
139 36 data->u = d->data_0->u;
140 }
141
142 template <typename Scalar>
143 36 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
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 36 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
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
 36 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 36 Data* d = static_cast<Data*>(data.get());
157 36 model_->torqueTransform(d->data_0, x, u);
158 36 d->Mtau = d->data_0->Mtau;
159 }
160
161 template <typename Scalar>
162 std::shared_ptr<ActuationDataAbstractTpl<Scalar> >
163 144 ActuationModelNumDiffTpl<Scalar>::createData() {
164
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 144 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 9280 ActuationModelNumDiffTpl<Scalar>::get_model() const {
179 9280 return model_;
180 }
181
182 template <typename Scalar>
183 54 const Scalar ActuationModelNumDiffTpl<Scalar>::get_disturbance() const {
184 54 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