GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residual-base.hxx
Date: 2025-01-16 08:47:40
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 boost::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 boost::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 boost::shared_ptr<ResidualDataAbstract>&,
44 ✗ const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
45
46 template <typename Scalar>
47 33280 void ResidualModelAbstractTpl<Scalar>::calc(
48 const boost::shared_ptr<ResidualDataAbstract>& data,
49 const Eigen::Ref<const VectorXs>& x) {
50
1/2
✓ Branch 2 taken 33280 times.
✗ Branch 3 not taken.
 33280 calc(data, x, unone_);
51 33280 }
52
53 template <typename Scalar>
54 ✗ void ResidualModelAbstractTpl<Scalar>::calcDiff(
55 const boost::shared_ptr<ResidualDataAbstract>&,
56 ✗ const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&) {}
57
58 template <typename Scalar>
59 1619 void ResidualModelAbstractTpl<Scalar>::calcDiff(
60 const boost::shared_ptr<ResidualDataAbstract>& data,
61 const Eigen::Ref<const VectorXs>& x) {
62
1/2
✓ Branch 2 taken 1619 times.
✗ Branch 3 not taken.
 1619 calcDiff(data, x, unone_);
63 1619 }
64
65 template <typename Scalar>
66 boost::shared_ptr<ResidualDataAbstractTpl<Scalar> >
67 99216 ResidualModelAbstractTpl<Scalar>::createData(
68 DataCollectorAbstract* const data) {
69 return boost::allocate_shared<ResidualDataAbstract>(
70
1/2
✓ Branch 2 taken 99216 times.
✗ Branch 3 not taken.
 198432 Eigen::aligned_allocator<ResidualDataAbstract>(), this, data);
71 }
72
73 template <typename Scalar>
74 35269 void ResidualModelAbstractTpl<Scalar>::calcCostDiff(
75 const boost::shared_ptr<CostDataAbstract>& cdata,
76 const boost::shared_ptr<ResidualDataAbstract>& rdata,
77 const boost::shared_ptr<ActivationDataAbstract>& adata,
78 const bool update_u) {
79 // This function computes the derivatives of the cost function based on a
80 // Gauss-Newton approximation
81
6/6
✓ Branch 0 taken 29313 times.
✓ Branch 1 taken 5956 times.
✓ Branch 2 taken 27705 times.
✓ Branch 3 taken 1608 times.
✓ Branch 4 taken 27168 times.
✓ Branch 5 taken 537 times.
 35269 const bool is_ru = u_dependent_ && nu_ != 0 && update_u;
82 35269 const std::size_t nv = state_->get_nv();
83
2/2
✓ Branch 0 taken 27168 times.
✓ Branch 1 taken 8101 times.
 35269 if (is_ru) {
84
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;
85
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;
86
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;
87 }
88
4/4
✓ Branch 0 taken 19546 times.
✓ Branch 1 taken 15723 times.
✓ Branch 2 taken 13698 times.
✓ Branch 3 taken 5848 times.
 35269 if (q_dependent_ && v_dependent_) {
89
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;
90
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;
91
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;
92
2/2
✓ Branch 0 taken 11382 times.
✓ Branch 1 taken 2316 times.
 13698 if (is_ru) {
93
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;
94 }
95
2/2
✓ Branch 0 taken 5848 times.
✓ Branch 1 taken 15723 times.
 21571 } else if (q_dependent_) {
96 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rq =
97
1/2
✓ Branch 2 taken 5848 times.
✗ Branch 3 not taken.
 5848 rdata->Rx.leftCols(nv);
98
5/10
✓ Branch 2 taken 5848 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5848 times.
✗ Branch 6 not taken.
✓ Branch 9 taken 5848 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 5848 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 5848 times.
✗ Branch 16 not taken.
 5848 cdata->Lx.head(nv).noalias() = Rq.transpose() * adata->Ar;
99
3/6
✓ Branch 2 taken 5848 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5848 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5848 times.
✗ Branch 9 not taken.
 5848 rdata->Arr_Rx.leftCols(nv).noalias() =
100
2/4
✓ Branch 3 taken 5848 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 5848 times.
✗ Branch 7 not taken.
 5848 adata->Arr.diagonal().asDiagonal() * Rq;
101
3/6
✓ Branch 2 taken 5848 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5848 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5848 times.
✗ Branch 9 not taken.
 5848 cdata->Lxx.topLeftCorner(nv, nv).noalias() =
102
3/6
✓ Branch 2 taken 5848 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 5848 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 5848 times.
✗ Branch 9 not taken.
 5848 Rq.transpose() * rdata->Arr_Rx.leftCols(nv);
103
2/2
✓ Branch 0 taken 63 times.
✓ Branch 1 taken 5785 times.
 5848 if (is_ru) {
104
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;
105 }
106
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 15723 times.
 15723 } else if (v_dependent_) {
107 Eigen::Block<MatrixXs, Eigen::Dynamic, Eigen::Dynamic, true> Rv =
108 ✗ rdata->Rx.rightCols(nv);
109 ✗ cdata->Lx.tail(nv).noalias() = Rv.transpose() * adata->Ar;
110 ✗ rdata->Arr_Rx.rightCols(nv).noalias() =
111 ✗ adata->Arr.diagonal().asDiagonal() * Rv;
112 ✗ cdata->Lxx.bottomRightCorner(nv, nv).noalias() =
113 ✗ Rv.transpose() * rdata->Arr_Rx.rightCols(nv);
114 ✗ if (is_ru) {
115 ✗ cdata->Lxu.bottomRows(nv).noalias() = Rv.transpose() * rdata->Arr_Ru;
116 }
117 }
118 35269 }
119
120 template <typename Scalar>
121 20 void ResidualModelAbstractTpl<Scalar>::print(std::ostream& os) const {
122
1/2
✓ Branch 3 taken 20 times.
✗ Branch 4 not taken.
 20 os << boost::core::demangle(typeid(*this).name());
123 20 }
124
125 template <typename Scalar>
126 const boost::shared_ptr<StateAbstractTpl<Scalar> >&
127 1912860 ResidualModelAbstractTpl<Scalar>::get_state() const {
128 1912860 return state_;
129 }
130
131 template <typename Scalar>
132 3679943 std::size_t ResidualModelAbstractTpl<Scalar>::get_nr() const {
133 3679943 return nr_;
134 }
135
136 template <typename Scalar>
137 1516934 std::size_t ResidualModelAbstractTpl<Scalar>::get_nu() const {
138 1516934 return nu_;
139 }
140
141 template <typename Scalar>
142 108443 bool ResidualModelAbstractTpl<Scalar>::get_q_dependent() const {
143 108443 return q_dependent_;
144 }
145
146 template <typename Scalar>
147 106541 bool ResidualModelAbstractTpl<Scalar>::get_v_dependent() const {
148 106541 return v_dependent_;
149 }
150
151 template <typename Scalar>
152 19444 bool ResidualModelAbstractTpl<Scalar>::get_u_dependent() const {
153 19444 return u_dependent_;
154 }
155
156 template <typename Scalar>
157 506 std::ostream& operator<<(std::ostream& os,
158 const ResidualModelAbstractTpl<Scalar>& model) {
159 506 model.print(os);
160 506 return os;
161 }
162
163 } // namespace crocoddyl
164