GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residual-base.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 63 77 81.8%
Branches: 71 172 41.3%
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 <boost/core/demangle.hpp>
10
11 namespace crocoddyl {
12
13 template <typename Scalar>
14 11278 ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl(
15 std::shared_ptr<StateAbstract> state, const std::size_t nr,
16 const std::size_t nu, const bool q_dependent, const bool v_dependent,
17 const bool u_dependent)
18 11278 : state_(state),
19 11278 nr_(nr),
20 11278 nu_(nu),
21
2/4
✓ Branch 1 taken 11278 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11278 times.
✗ Branch 5 not taken.
 11278 unone_(VectorXs::Zero(nu)),
22 11278 q_dependent_(q_dependent),
23 11278 v_dependent_(v_dependent),
24 22556 u_dependent_(u_dependent) {}
25
26 template <typename Scalar>
27 45 ResidualModelAbstractTpl<Scalar>::ResidualModelAbstractTpl(
28 std::shared_ptr<StateAbstract> state, const std::size_t nr,
29 const bool q_dependent, const bool v_dependent, const bool u_dependent)
30 45 : state_(state),
31 45 nr_(nr),
32
1/2
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
 45 nu_(state->get_nv()),
33
3/6
✓ Branch 2 taken 45 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 45 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 45 times.
✗ Branch 9 not taken.
 45 unone_(VectorXs::Zero(state->get_nv())),
34 45 q_dependent_(q_dependent),
35 45 v_dependent_(v_dependent),
36 90 u_dependent_(u_dependent) {}
37
38 template <typename Scalar>
39 ✗ void ResidualModelAbstractTpl<Scalar>::calc(
40 const std::shared_ptr<ResidualDataAbstract>&,
41 ✗ const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
42
43 template <typename Scalar>
44 32470 void ResidualModelAbstractTpl<Scalar>::calc(
45 const std::shared_ptr<ResidualDataAbstract>& data,
46 const Eigen::Ref<const VectorXs>& x) {
47
1/2
✓ Branch 2 taken 32470 times.
✗ Branch 3 not taken.
 32470 calc(data, x, unone_);
48 32470 }
49
50 template <typename Scalar>
51 ✗ void ResidualModelAbstractTpl<Scalar>::calcDiff(
52 const std::shared_ptr<ResidualDataAbstract>&,
53 ✗ const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
54
55 template <typename Scalar>
56 1607 void ResidualModelAbstractTpl<Scalar>::calcDiff(
57 const std::shared_ptr<ResidualDataAbstract>& data,
58 const Eigen::Ref<const VectorXs>& x) {
59
1/2
✓ Branch 2 taken 1607 times.
✗ Branch 3 not taken.
 1607 calcDiff(data, x, unone_);
60 1607 }
61
62 template <typename Scalar>
63 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
64 98878 ResidualModelAbstractTpl<Scalar>::createData(
65 DataCollectorAbstract* const data) {
66 return std::allocate_shared<ResidualDataAbstract>(
67
1/2
✓ Branch 2 taken 98878 times.
✗ Branch 3 not taken.
 197756 Eigen::aligned_allocator<ResidualDataAbstract>(), this, data);
68 }
69
70 template <typename Scalar>
71 35013 void ResidualModelAbstractTpl<Scalar>::calcCostDiff(
72 const std::shared_ptr<CostDataAbstract>& cdata,
73 const std::shared_ptr<ResidualDataAbstract>& rdata,
74 const std::shared_ptr<ActivationDataAbstract>& adata, const bool update_u) {
75 // This function computes the derivatives of the cost function based on a
76 // Gauss-Newton approximation
77
6/6
✓ Branch 0 taken 29313 times.
✓ Branch 1 taken 5700 times.
✓ Branch 2 taken 27705 times.
✓ Branch 3 taken 1608 times.
✓ Branch 4 taken 27168 times.
✓ Branch 5 taken 537 times.
 35013 const bool is_ru = u_dependent_ && nu_ != 0 && update_u;
78 35013 const std::size_t nv = state_->get_nv();
79
2/2
✓ Branch 0 taken 27168 times.
✓ Branch 1 taken 7845 times.
 35013 if (is_ru) {
80
3/6
✓ Branch 4 taken 27168 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27168 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27168 times.
✗ Branch 12 not taken.
 27168 cdata->Lu.noalias() = rdata->Ru.transpose() * adata->Ar;
81
3/6
✓ Branch 5 taken 27168 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 27168 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 27168 times.
✗ Branch 13 not taken.
 27168 rdata->Arr_Ru.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Ru;
82
3/6
✓ Branch 4 taken 27168 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 27168 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 27168 times.
✗ Branch 12 not taken.
 27168 cdata->Luu.noalias() = rdata->Ru.transpose() * rdata->Arr_Ru;
83 }
84
4/4
✓ Branch 0 taken 19290 times.
✓ Branch 1 taken 15723 times.
✓ Branch 2 taken 13714 times.
✓ Branch 3 taken 5576 times.
 35013 if (q_dependent_ && v_dependent_) {
85
3/6
✓ Branch 4 taken 13714 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 13714 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13714 times.
✗ Branch 12 not taken.
 13714 cdata->Lx.noalias() = rdata->Rx.transpose() * adata->Ar;
86
3/6
✓ Branch 5 taken 13714 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 13714 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13714 times.
✗ Branch 13 not taken.
 13714 rdata->Arr_Rx.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Rx;
87
3/6
✓ Branch 4 taken 13714 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 13714 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13714 times.
✗ Branch 12 not taken.
 13714 cdata->Lxx.noalias() = rdata->Rx.transpose() * rdata->Arr_Rx;
88
2/2
✓ Branch 0 taken 11382 times.
✓ Branch 1 taken 2332 times.
 13714 if (is_ru) {
89
3/6
✓ Branch 4 taken 11382 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 11382 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 11382 times.
✗ Branch 12 not taken.
 11382 cdata->Lxu.noalias() = rdata->Rx.transpose() * rdata->Arr_Ru;
90 }
91
2/2
✓ Branch 0 taken 5576 times.
✓ Branch 1 taken 15723 times.
 21299 } else if (q_dependent_) {
92 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
93
1/2
✓ Branch 2 taken 5576 times.
✗ Branch 3 not taken.
 5576 rdata->Rx.leftCols(nv);
94
5/10
✓ Branch 2 taken 5576 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5576 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 5576 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5576 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5576 times.
✗ Branch 16 not taken.
 5576 cdata->Lx.head(nv).noalias() = Rq.transpose() * adata->Ar;
95
3/6
✓ Branch 2 taken 5576 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5576 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5576 times.
✗ Branch 9 not taken.
 5576 rdata->Arr_Rx.leftCols(nv).noalias() =
96
2/4
✓ Branch 3 taken 5576 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5576 times.
✗ Branch 7 not taken.
 5576 adata->Arr.diagonal().asDiagonal() * Rq;
97
3/6
✓ Branch 2 taken 5576 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5576 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5576 times.
✗ Branch 9 not taken.
 5576 cdata->Lxx.topLeftCorner(nv, nv).noalias() =
98
3/6
✓ Branch 2 taken 5576 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5576 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5576 times.
✗ Branch 9 not taken.
 5576 Rq.transpose() * rdata->Arr_Rx.leftCols(nv);
99
2/2
✓ Branch 0 taken 63 times.
✓ Branch 1 taken 5513 times.
 5576 if (is_ru) {
100
5/10
✓ Branch 2 taken 63 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 63 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 63 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 63 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 63 times.
✗ Branch 16 not taken.
 63 cdata->Lxu.topRows(nv).noalias() = Rq.transpose() * rdata->Arr_Ru;
101 }
102
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15723 times.
 15723 } else if (v_dependent_) {
103 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv =
104 ✗ rdata->Rx.rightCols(nv);
105 ✗ cdata->Lx.tail(nv).noalias() = Rv.transpose() * adata->Ar;
106 ✗ rdata->Arr_Rx.rightCols(nv).noalias() =
107 ✗ adata->Arr.diagonal().asDiagonal() * Rv;
108 ✗ cdata->Lxx.bottomRightCorner(nv, nv).noalias() =
109 ✗ Rv.transpose() * rdata->Arr_Rx.rightCols(nv);
110 ✗ if (is_ru) {
111 ✗ cdata->Lxu.bottomRows(nv).noalias() = Rv.transpose() * rdata->Arr_Ru;
112 }
113 }
114 35013 }
115
116 template <typename Scalar>
117 ✗ void ResidualModelAbstractTpl<Scalar>::print(std::ostream& os) const {
118 ✗ os << boost::core::demangle(typeid(*this).name());
119 }
120
121 template <typename Scalar>
122 const std::shared_ptr<StateAbstractTpl<Scalar> >&
123 1900404 ResidualModelAbstractTpl<Scalar>::get_state() const {
124 1900404 return state_;
125 }
126
127 template <typename Scalar>
128 3660989 std::size_t ResidualModelAbstractTpl<Scalar>::get_nr() const {
129 3660989 return nr_;
130 }
131
132 template <typename Scalar>
133 1508826 std::size_t ResidualModelAbstractTpl<Scalar>::get_nu() const {
134 1508826 return nu_;
135 }
136
137 template <typename Scalar>
138 108457 bool ResidualModelAbstractTpl<Scalar>::get_q_dependent() const {
139 108457 return q_dependent_;
140 }
141
142 template <typename Scalar>
143 106553 bool ResidualModelAbstractTpl<Scalar>::get_v_dependent() const {
144 106553 return v_dependent_;
145 }
146
147 template <typename Scalar>
148 19446 bool ResidualModelAbstractTpl<Scalar>::get_u_dependent() const {
149 19446 return u_dependent_;
150 }
151
152 template <typename Scalar>
153 486 std::ostream& operator<<(std::ostream& os,
154 const ResidualModelAbstractTpl<Scalar>& model) {
155 486 model.print(os);
156 486 return os;
157 }
158
159 } // namespace crocoddyl
160