GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residual-base.hxx
Date: 2025-02-24 23:41:29
Exec Total Coverage
Lines: 67 79 84.8%
Branches: 70 168 41.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-2023, 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 11382 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 11382 : state_(state),
19 11382 nr_(nr),
20 11382 nu_(nu),
21
2/4
✓ Branch 1 taken 11382 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11382 times.
✗ Branch 5 not taken.
 11382 unone_(VectorXs::Zero(nu)),
22 11382 q_dependent_(q_dependent),
23 11382 v_dependent_(v_dependent),
24 22764 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 45 nu_(state->get_nv()),
33
2/4
✓ Branch 3 taken 45 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 45 times.
✗ Branch 7 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 22918 ResidualModelAbstractTpl<Scalar>::~ResidualModelAbstractTpl() {}
40
41 template <typename Scalar>
42 ✗ void ResidualModelAbstractTpl<Scalar>::calc(
43 const std::shared_ptr<ResidualDataAbstract>&,
44 ✗ const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
45
46 template <typename Scalar>
47 33266 void ResidualModelAbstractTpl<Scalar>::calc(
48 const std::shared_ptr<ResidualDataAbstract>& data,
49 const Eigen::Ref<const VectorXs>& x) {
50
1/2
✓ Branch 2 taken 33266 times.
✗ Branch 3 not taken.
 33266 calc(data, x, unone_);
51 33266 }
52
53 template <typename Scalar>
54 ✗ void ResidualModelAbstractTpl<Scalar>::calcDiff(
55 const std::shared_ptr<ResidualDataAbstract>&,
56 ✗ const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
57
58 template <typename Scalar>
59 1597 void ResidualModelAbstractTpl<Scalar>::calcDiff(
60 const std::shared_ptr<ResidualDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>& x) {
62
1/2
✓ Branch 2 taken 1597 times.
✗ Branch 3 not taken.
 1597 calcDiff(data, x, unone_);
63 1597 }
64
65 template <typename Scalar>
66 std::shared_ptr<ResidualDataAbstractTpl<Scalar> >
67 99266 ResidualModelAbstractTpl<Scalar>::createData(
68 DataCollectorAbstract* const data) {
69 return std::allocate_shared<ResidualDataAbstract>(
70
1/2
✓ Branch 2 taken 99266 times.
✗ Branch 3 not taken.
 198532 Eigen::aligned_allocator<ResidualDataAbstract>(), this, data);
71 }
72
73 template <typename Scalar>
74 34865 void ResidualModelAbstractTpl<Scalar>::calcCostDiff(
75 const std::shared_ptr<CostDataAbstract>& cdata,
76 const std::shared_ptr<ResidualDataAbstract>& rdata,
77 const std::shared_ptr<ActivationDataAbstract>& adata, const bool update_u) {
78 // This function computes the derivatives of the cost function based on a
79 // Gauss-Newton approximation
80
6/6
✓ Branch 0 taken 29313 times.
✓ Branch 1 taken 5552 times.
✓ Branch 2 taken 27705 times.
✓ Branch 3 taken 1608 times.
✓ Branch 4 taken 27168 times.
✓ Branch 5 taken 537 times.
 34865 const bool is_ru = u_dependent_ && nu_ != 0 && update_u;
81 34865 const std::size_t nv = state_->get_nv();
82
2/2
✓ Branch 0 taken 27168 times.
✓ Branch 1 taken 7697 times.
 34865 if (is_ru) {
83
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;
84
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;
85
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;
86 }
87
4/4
✓ Branch 0 taken 19142 times.
✓ Branch 1 taken 15723 times.
✓ Branch 2 taken 13698 times.
✓ Branch 3 taken 5444 times.
 34865 if (q_dependent_ && v_dependent_) {
88
3/6
✓ Branch 4 taken 13698 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 13698 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13698 times.
✗ Branch 12 not taken.
 13698 cdata->Lx.noalias() = rdata->Rx.transpose() * adata->Ar;
89
3/6
✓ Branch 5 taken 13698 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 13698 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 13698 times.
✗ Branch 13 not taken.
 13698 rdata->Arr_Rx.noalias() = adata->Arr.diagonal().asDiagonal() * rdata->Rx;
90
3/6
✓ Branch 4 taken 13698 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 13698 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 13698 times.
✗ Branch 12 not taken.
 13698 cdata->Lxx.noalias() = rdata->Rx.transpose() * rdata->Arr_Rx;
91
2/2
✓ Branch 0 taken 11382 times.
✓ Branch 1 taken 2316 times.
 13698 if (is_ru) {
92
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;
93 }
94
2/2
✓ Branch 0 taken 5444 times.
✓ Branch 1 taken 15723 times.
 21167 } else if (q_dependent_) {
95 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
96
1/2
✓ Branch 2 taken 5444 times.
✗ Branch 3 not taken.
 5444 rdata->Rx.leftCols(nv);
97
5/10
✓ Branch 2 taken 5444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5444 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 5444 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5444 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5444 times.
✗ Branch 16 not taken.
 5444 cdata->Lx.head(nv).noalias() = Rq.transpose() * adata->Ar;
98
3/6
✓ Branch 2 taken 5444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5444 times.
✗ Branch 9 not taken.
 5444 rdata->Arr_Rx.leftCols(nv).noalias() =
99
2/4
✓ Branch 3 taken 5444 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5444 times.
✗ Branch 7 not taken.
 5444 adata->Arr.diagonal().asDiagonal() * Rq;
100
3/6
✓ Branch 2 taken 5444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5444 times.
✗ Branch 9 not taken.
 5444 cdata->Lxx.topLeftCorner(nv, nv).noalias() =
101
3/6
✓ Branch 2 taken 5444 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5444 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5444 times.
✗ Branch 9 not taken.
 5444 Rq.transpose() * rdata->Arr_Rx.leftCols(nv);
102
2/2
✓ Branch 0 taken 63 times.
✓ Branch 1 taken 5381 times.
 5444 if (is_ru) {
103
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;
104 }
105
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15723 times.
 15723 } else if (v_dependent_) {
106 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv =
107 ✗ rdata->Rx.rightCols(nv);
108 ✗ cdata->Lx.tail(nv).noalias() = Rv.transpose() * adata->Ar;
109 ✗ rdata->Arr_Rx.rightCols(nv).noalias() =
110 ✗ adata->Arr.diagonal().asDiagonal() * Rv;
111 ✗ cdata->Lxx.bottomRightCorner(nv, nv).noalias() =
112 ✗ Rv.transpose() * rdata->Arr_Rx.rightCols(nv);
113 ✗ if (is_ru) {
114 ✗ cdata->Lxu.bottomRows(nv).noalias() = Rv.transpose() * rdata->Arr_Ru;
115 }
116 }
117 34865 }
118
119 template <typename Scalar>
120 20 void ResidualModelAbstractTpl<Scalar>::print(std::ostream& os) const {
121
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 os << boost::core::demangle(typeid(*this).name());
122 20 }
123
124 template <typename Scalar>
125 const std::shared_ptr<StateAbstractTpl<Scalar> >&
126 1913126 ResidualModelAbstractTpl<Scalar>::get_state() const {
127 1913126 return state_;
128 }
129
130 template <typename Scalar>
131 3680507 std::size_t ResidualModelAbstractTpl<Scalar>::get_nr() const {
132 3680507 return nr_;
133 }
134
135 template <typename Scalar>
136 1517186 std::size_t ResidualModelAbstractTpl<Scalar>::get_nu() const {
137 1517186 return nu_;
138 }
139
140 template <typename Scalar>
141 108377 bool ResidualModelAbstractTpl<Scalar>::get_q_dependent() const {
142 108377 return q_dependent_;
143 }
144
145 template <typename Scalar>
146 106472 bool ResidualModelAbstractTpl<Scalar>::get_v_dependent() const {
147 106472 return v_dependent_;
148 }
149
150 template <typename Scalar>
151 19450 bool ResidualModelAbstractTpl<Scalar>::get_u_dependent() const {
152 19450 return u_dependent_;
153 }
154
155 template <typename Scalar>
156 506 std::ostream& operator<<(std::ostream& os,
157 const ResidualModelAbstractTpl<Scalar>& model) {
158 506 model.print(os);
159 506 return os;
160 }
161
162 } // namespace crocoddyl
163