GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/numdiff/control.hxx
Date: 2025-01-30 11:01:55
Exec Total Coverage
Lines: 43 63 68.3%
Branches: 29 124 23.4%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2023, LAAS-CNRS, University of Edinburgh, New York
5 // University,
6 // Heriot-Watt University
7 // Max Planck Gesellschaft, University of Trento
8 // Copyright note valid unless otherwise stated in individual files.
9 // All rights reserved.
10 ///////////////////////////////////////////////////////////////////////////////
11
12 #include "crocoddyl/core/utils/exception.hpp"
13
14 namespace crocoddyl {
15
16 template <typename Scalar>
17 12 ControlParametrizationModelNumDiffTpl<
18 Scalar>::ControlParametrizationModelNumDiffTpl(std::shared_ptr<Base> model)
19 : Base(model->get_nw(), model->get_nu()),
20 12 model_(model),
21 12 e_jac_(std::sqrt(2.0 * std::numeric_limits<Scalar>::epsilon())) {}
22
23 template <typename Scalar>
24 24 ControlParametrizationModelNumDiffTpl<
25 24 Scalar>::~ControlParametrizationModelNumDiffTpl() {}
26
27 template <typename Scalar>
28 12 void ControlParametrizationModelNumDiffTpl<Scalar>::calc(
29 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
30 const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
31 12 std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data);
32
1/2
✓ Branch 3 taken 12 times.
✗ Branch 4 not taken.
 12 model_->calc(data_nd->data_0, t, u);
33 12 }
34
35 template <typename Scalar>
36 12 void ControlParametrizationModelNumDiffTpl<Scalar>::calcDiff(
37 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
38 const Scalar t, const Eigen::Ref<const VectorXs>& u) const {
39 12 std::shared_ptr<Data> data_nd = std::static_pointer_cast<Data>(data);
40
1/2
✓ Branch 4 taken 12 times.
✗ Branch 5 not taken.
 12 data->w = data_nd->data_0->w;
41
42
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 data_nd->du.setZero();
43
1/2
✓ Branch 1 taken 12 times.
✗ Branch 2 not taken.
 12 const Scalar uh_jac = e_jac_ * std::max(1., u.norm());
44
2/2
✓ Branch 2 taken 270 times.
✓ Branch 3 taken 12 times.
 282 for (std::size_t i = 0; i < model_->get_nu(); ++i) {
45
1/2
✓ Branch 2 taken 270 times.
✗ Branch 3 not taken.
 270 data_nd->du(i) += uh_jac;
46
3/6
✓ Branch 3 taken 270 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 270 times.
✗ Branch 7 not taken.
✓ Branch 11 taken 270 times.
✗ Branch 12 not taken.
 270 model_->calc(data_nd->data_u[i], t, u + data_nd->du);
47
3/6
✓ Branch 5 taken 270 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 270 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 270 times.
✗ Branch 13 not taken.
 270 data->dw_du.col(i) = data_nd->data_u[i]->w - data->w;
48
1/2
✓ Branch 2 taken 270 times.
✗ Branch 3 not taken.
 270 data_nd->du(i) = 0.;
49 }
50
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 data->dw_du /= uh_jac;
51 12 }
52
53 template <typename Scalar>
54 std::shared_ptr<ControlParametrizationDataAbstractTpl<Scalar> >
55 12 ControlParametrizationModelNumDiffTpl<Scalar>::createData() {
56
1/2
✓ Branch 2 taken 12 times.
✗ Branch 3 not taken.
 12 return std::allocate_shared<Data>(Eigen::aligned_allocator<Data>(), this);
57 }
58
59 template <typename Scalar>
60 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::params(
61 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
62 const Scalar t, const Eigen::Ref<const VectorXs>& w) const {
63 ✗ model_->params(data, t, w);
64 }
65
66 template <typename Scalar>
67 ✗ void ControlParametrizationModelNumDiffTpl<Scalar>::convertBounds(
68 const Eigen::Ref<const VectorXs>& w_lb,
69 const Eigen::Ref<const VectorXs>& w_ub, Eigen::Ref<VectorXs> u_lb,
70 Eigen::Ref<VectorXs> u_ub) const {
71 ✗ model_->convertBounds(w_lb, w_ub, u_lb, u_ub);
72 }
73
74 template <typename Scalar>
75 4 void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyByJacobian(
76 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
77 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
78 const AssignmentOp op) const {
79
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 4 assert_pretty(is_a_AssignmentOp(op),
80 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
81
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 MatrixXs J(nw_, nu_);
82
1/4
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4 switch (op) {
83 4 case setto:
84
3/6
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
 4 out.noalias() = A * data->dw_du;
85 4 break;
86 ✗ case addto:
87 ✗ out.noalias() += A * data->dw_du;
88 ✗ break;
89 ✗ case rmfrom:
90 ✗ out.noalias() -= A * data->dw_du;
91 ✗ break;
92 ✗ default:
93 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
94 break;
95 }
96 4 }
97
98 template <typename Scalar>
99 4 void ControlParametrizationModelNumDiffTpl<Scalar>::multiplyJacobianTransposeBy(
100 const std::shared_ptr<ControlParametrizationDataAbstract>& data,
101 const Eigen::Ref<const MatrixXs>& A, Eigen::Ref<MatrixXs> out,
102 const AssignmentOp op) const {
103
1/10
✗ Branch 1 not taken.
✓ Branch 2 taken 4 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 11 not taken.
✗ Branch 12 not taken.
✗ Branch 14 not taken.
✗ Branch 15 not taken.
 4 assert_pretty(is_a_AssignmentOp(op),
104 ("op must be one of the AssignmentOp {settop, addto, rmfrom}"));
105
1/2
✓ Branch 1 taken 4 times.
✗ Branch 2 not taken.
 4 MatrixXs J(nw_, nu_);
106
1/4
✓ Branch 0 taken 4 times.
✗ Branch 1 not taken.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 4 switch (op) {
107 4 case setto:
108
4/8
✓ Branch 2 taken 4 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 4 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 4 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 4 times.
✗ Branch 12 not taken.
 4 out.noalias() = data->dw_du.transpose() * A;
109 4 break;
110 ✗ case addto:
111 ✗ out.noalias() += data->dw_du.transpose() * A;
112 ✗ break;
113 ✗ case rmfrom:
114 ✗ out.noalias() -= data->dw_du.transpose() * A;
115 ✗ break;
116 ✗ default:
117 ✗ throw_pretty("Invalid argument: allowed operators: setto, addto, rmfrom");
118 break;
119 }
120 4 }
121
122 template <typename Scalar>
123 const std::shared_ptr<ControlParametrizationModelAbstractTpl<Scalar> >&
124 294 ControlParametrizationModelNumDiffTpl<Scalar>::get_model() const {
125 294 return model_;
126 }
127
128 template <typename Scalar>
129 12 const Scalar ControlParametrizationModelNumDiffTpl<Scalar>::get_disturbance()
130 const {
131 12 return e_jac_;
132 }
133
134 template <typename Scalar>
135 void ControlParametrizationModelNumDiffTpl<Scalar>::set_disturbance(
136 const Scalar disturbance) {
137 if (disturbance < 0.) {
138 throw_pretty("Invalid argument: " << "Disturbance constant is positive");
139 }
140 e_jac_ = disturbance;
141 }
142
143 } // namespace crocoddyl
144