GCC Code Coverage Report

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