GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/actuation.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 76 86 88.4%
Branches: 39 232 16.8%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2024, University of Edinburgh, LAAS-CNRS
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
7 ///////////////////////////////////////////////////////////////////////////////
8
9 #include "crocoddyl/core/numdiff/actuation.hpp"
10 #include "crocoddyl/core/utils/exception.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 72 ActuationModelNumDiffTpl<Scalar>::ActuationModelNumDiffTpl(
16 boost::shared_ptr<Base> model)
17 : Base(model->get_state(), model->get_nu()),
18 72 model_(model),
19 72 e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
20
21 template <typename Scalar>
22 144 ActuationModelNumDiffTpl<Scalar>::~ActuationModelNumDiffTpl() {}
23
24 template <typename Scalar>
25 36 void ActuationModelNumDiffTpl<Scalar>::calc(
26 const boost::shared_ptr<ActuationDataAbstract>& data,
27 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
28
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 36 times.
 36 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
29 ✗ throw_pretty("Invalid argument: "
30 << "x has wrong dimension (it should be " +
31 std::to_string(model_->get_state()->get_nx()) + ")");
32 }
33
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 36 if (static_cast<std::size_t>(u.size()) != nu_) {
34 ✗ throw_pretty(
35 "Invalid argument: " << "u has wrong dimension (it should be " +
36 std::to_string(nu_) + ")");
37 }
38 36 Data* d = static_cast<Data*>(data.get());
39 36 model_->calc(d->data_0, x, u);
40 36 data->tau = d->data_0->tau;
41 36 }
42
43 template <typename Scalar>
44 18 void ActuationModelNumDiffTpl<Scalar>::calc(
45 const boost::shared_ptr<ActuationDataAbstract>& data,
46 const Eigen::Ref<const VectorXs>& x) {
47
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 18 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
48 ✗ throw_pretty("Invalid argument: "
49 << "x has wrong dimension (it should be " +
50 std::to_string(model_->get_state()->get_nx()) + ")");
51 }
52 18 Data* d = static_cast<Data*>(data.get());
53 18 model_->calc(d->data_0, x);
54 18 data->tau = d->data_0->tau;
55 18 }
56
57 template <typename Scalar>
58 36 void ActuationModelNumDiffTpl<Scalar>::calcDiff(
59 const boost::shared_ptr<ActuationDataAbstract>& data,
60 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
61
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 36 times.
 36 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
62 ✗ throw_pretty("Invalid argument: "
63 << "x has wrong dimension (it should be " +
64 std::to_string(model_->get_state()->get_nx()) + ")");
65 }
66
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 36 times.
 36 if (static_cast<std::size_t>(u.size()) != nu_) {
67 ✗ throw_pretty(
68 "Invalid argument: " << "u has wrong dimension (it should be " +
69 std::to_string(nu_) + ")");
70 }
71 36 Data* d = static_cast<Data*>(data.get());
72 36 const VectorXs& tau0 = d->data_0->tau;
73 36 d->du.setZero();
74
75 // Computing the d actuation(x,u) / dx
76
3/6
✓ Branch 8 taken 36 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 36 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 36 times.
✗ Branch 15 not taken.
 36 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
77 36 d->x_norm = d->dx.norm();
78 36 d->dx.setZero();
79 36 d->xh_jac = e_jac_ * std::max(1., d->x_norm);
80
2/2
✓ Branch 4 taken 1432 times.
✓ Branch 5 taken 36 times.
 1468 for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
81 1432 d->dx(ix) = d->xh_jac;
82
2/4
✓ Branch 5 taken 1432 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 1432 times.
✗ Branch 9 not taken.
 1432 model_->get_state()->integrate(x, d->dx, d->xp);
83
1/2
✓ Branch 4 taken 1432 times.
✗ Branch 5 not taken.
 1432 model_->calc(d->data_x[ix], d->xp, u);
84
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.
 1432 d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
85 1432 d->dx(ix) = 0.;
86 }
87
88 // Computing the d actuation(x,u) / du
89 36 d->uh_jac = e_jac_ * std::max(1., u.norm());
90
2/2
✓ Branch 2 taken 672 times.
✓ Branch 3 taken 36 times.
 708 for (unsigned iu = 0; iu < model_->get_nu(); ++iu) {
91 672 d->du(iu) = d->uh_jac;
92
2/4
✓ Branch 3 taken 672 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 672 times.
✗ Branch 8 not taken.
 672 model_->calc(d->data_u[iu], x, u + d->du);
93
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.
 672 d->dtau_du.col(iu) = (d->data_u[iu]->tau - tau0) / d->uh_jac;
94 672 d->du(iu) = 0.;
95 }
96 36 }
97
98 template <typename Scalar>
99 18 void ActuationModelNumDiffTpl<Scalar>::calcDiff(
100 const boost::shared_ptr<ActuationDataAbstract>& data,
101 const Eigen::Ref<const VectorXs>& x) {
102
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 18 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
103 ✗ throw_pretty("Invalid argument: "
104 << "x has wrong dimension (it should be " +
105 std::to_string(model_->get_state()->get_nx()) + ")");
106 }
107 18 Data* d = static_cast<Data*>(data.get());
108 18 const VectorXs& tau0 = d->data_0->tau;
109 18 d->dx.setZero();
110
111 // Computing the d actuation(x,u) / dx
112
3/6
✓ Branch 8 taken 18 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 18 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 18 times.
✗ Branch 15 not taken.
 18 model_->get_state()->diff(model_->get_state()->zero(), x, d->dx);
113 18 d->x_norm = d->dx.norm();
114 18 d->dx.setZero();
115 18 d->xh_jac = e_jac_ * std::max(1., d->x_norm);
116
2/2
✓ Branch 4 taken 716 times.
✓ Branch 5 taken 18 times.
 734 for (std::size_t ix = 0; ix < model_->get_state()->get_ndx(); ++ix) {
117 716 d->dx(ix) = d->xh_jac;
118
2/4
✓ Branch 5 taken 716 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 716 times.
✗ Branch 9 not taken.
 716 model_->get_state()->integrate(x, d->dx, d->xp);
119 716 model_->calc(d->data_x[ix], d->xp);
120
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.
 716 d->dtau_dx.col(ix) = (d->data_x[ix]->tau - tau0) / d->xh_jac;
121 716 d->dx(ix) = 0.;
122 }
123 18 }
124
125 template <typename Scalar>
126 18 void ActuationModelNumDiffTpl<Scalar>::commands(
127 const boost::shared_ptr<ActuationDataAbstract>& data,
128 const Eigen::Ref<const VectorXs>& x,
129 const Eigen::Ref<const VectorXs>& tau) {
130
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 18 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
131 ✗ throw_pretty("Invalid argument: "
132 << "x has wrong dimension (it should be " +
133 std::to_string(model_->get_state()->get_nx()) + ")");
134 }
135
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 18 if (static_cast<std::size_t>(tau.size()) != model_->get_state()->get_nv()) {
136 ✗ throw_pretty("Invalid argument: "
137 << "tau has wrong dimension (it should be " +
138 std::to_string(model_->get_state()->get_nv()) + ")");
139 }
140 18 Data* d = static_cast<Data*>(data.get());
141 18 model_->commands(d->data_0, x, tau);
142 18 data->u = d->data_0->u;
143 18 }
144
145 template <typename Scalar>
146 18 void ActuationModelNumDiffTpl<Scalar>::torqueTransform(
147 const boost::shared_ptr<ActuationDataAbstract>& data,
148 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) {
149
1/2
✗ Branch 5 not taken.
✓ Branch 6 taken 18 times.
 18 if (static_cast<std::size_t>(x.size()) != model_->get_state()->get_nx()) {
150 ✗ throw_pretty("Invalid argument: "
151 << "x has wrong dimension (it should be " +
152 std::to_string(model_->get_state()->get_nx()) + ")");
153 }
154
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 18 times.
 18 if (static_cast<std::size_t>(u.size()) != nu_) {
155 ✗ throw_pretty(
156 "Invalid argument: " << "u has wrong dimension (it should be " +
157 std::to_string(nu_) + ")");
158 }
159 18 Data* d = static_cast<Data*>(data.get());
160 18 model_->torqueTransform(d->data_0, x, u);
161 18 d->Mtau = d->data_0->Mtau;
162 18 }
163
164 template <typename Scalar>
165 boost::shared_ptr<ActuationDataAbstractTpl<Scalar> >
166 72 ActuationModelNumDiffTpl<Scalar>::createData() {
167
1/2
✓ Branch 2 taken 72 times.
✗ Branch 3 not taken.
 72 return boost::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
168 }
169
170 template <typename Scalar>
171 const boost::shared_ptr<ActuationModelAbstractTpl<Scalar> >&
172 4640 ActuationModelNumDiffTpl<Scalar>::get_model() const {
173 4640 return model_;
174 }
175
176 template <typename Scalar>
177 54 const Scalar ActuationModelNumDiffTpl<Scalar>::get_disturbance() const {
178 54 return e_jac_;
179 }
180
181 template <typename Scalar>
182 void ActuationModelNumDiffTpl<Scalar>::set_disturbance(
183 const Scalar disturbance) {
184 if (disturbance < 0.) {
185 throw_pretty("Invalid argument: " << "Disturbance constant is positive");
186 }
187 e_jac_ = disturbance;
188 }
189
190 } // namespace crocoddyl
191