GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/residuals/state.hxx
Date: 2025-01-16 08:47:40
Exec Total Coverage
Lines: 58 71 81.7%
Branches: 41 174 23.6%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2022-2023, LAAS-CNRS, University of Edinburgh,
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
8 ///////////////////////////////////////////////////////////////////////////////
9
10 #include "crocoddyl/core/utils/exception.hpp"
11 #include "crocoddyl/multibody/residuals/state.hpp"
12
13 namespace crocoddyl {
14
15 template <typename Scalar>
16 1486 ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
17 boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref,
18 const std::size_t nu)
19
2/4
✓ Branch 4 taken 1486 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 1486 times.
✗ Branch 9 not taken.
 1486 : Base(state, state->get_ndx(), nu, true, true, false), xref_(xref) {
20
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1486 times.
 1486 if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
21 ✗ throw_pretty(
22 "Invalid argument: " << "xref has wrong dimension (it should be " +
23 std::to_string(state_->get_nx()) + ")");
24 }
25 // Define the pinocchio model for the multibody state case
26 1486 const boost::shared_ptr<StateMultibody>& s =
27 boost::dynamic_pointer_cast<StateMultibody>(state);
28
1/2
✓ Branch 1 taken 1486 times.
✗ Branch 2 not taken.
 1486 if (s) {
29 1486 pin_model_ = s->get_pinocchio();
30 }
31 1486 }
32
33 template <typename Scalar>
34 ✗ ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
35 boost::shared_ptr<typename Base::StateAbstract> state, const VectorXs& xref)
36 ✗ : Base(state, state->get_ndx(), true, true, false), xref_(xref) {
37 ✗ if (static_cast<std::size_t>(xref_.size()) != state_->get_nx()) {
38 ✗ throw_pretty(
39 "Invalid argument: " << "xref has wrong dimension (it should be " +
40 std::to_string(state_->get_nx()) + ")");
41 }
42 // Define the pinocchio model for the multibody state case
43 ✗ const boost::shared_ptr<StateMultibody>& s =
44 boost::dynamic_pointer_cast<StateMultibody>(state);
45 ✗ if (s) {
46 ✗ pin_model_ = s->get_pinocchio();
47 }
48 }
49
50 template <typename Scalar>
51 3 ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
52 boost::shared_ptr<typename Base::StateAbstract> state, const std::size_t nu)
53 : Base(state, state->get_ndx(), nu, true, true, false),
54
2/4
✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 3 times.
✗ Branch 10 not taken.
 3 xref_(state->zero()) {
55 // Define the pinocchio model for the multibody state case
56 3 const boost::shared_ptr<StateMultibody>& s =
57 boost::dynamic_pointer_cast<StateMultibody>(state);
58
1/2
✓ Branch 1 taken 3 times.
✗ Branch 2 not taken.
 3 if (s) {
59 3 pin_model_ = s->get_pinocchio();
60 }
61 3 }
62
63 template <typename Scalar>
64 10 ResidualModelStateTpl<Scalar>::ResidualModelStateTpl(
65 boost::shared_ptr<typename Base::StateAbstract> state)
66
2/4
✓ Branch 4 taken 10 times.
✗ Branch 5 not taken.
✓ Branch 9 taken 10 times.
✗ Branch 10 not taken.
 10 : Base(state, state->get_ndx(), true, true, false), xref_(state->zero()) {
67 // Define the pinocchio model for the multibody state case
68 10 const boost::shared_ptr<StateMultibody>& s =
69 boost::dynamic_pointer_cast<StateMultibody>(state);
70
1/2
✓ Branch 1 taken 10 times.
✗ Branch 2 not taken.
 10 if (s) {
71 10 pin_model_ = s->get_pinocchio();
72 }
73 10 }
74
75 template <typename Scalar>
76 3002 ResidualModelStateTpl<Scalar>::~ResidualModelStateTpl() {}
77
78 template <typename Scalar>
79 87689 void ResidualModelStateTpl<Scalar>::calc(
80 const boost::shared_ptr<ResidualDataAbstract>& data,
81 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
82
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 87689 times.
 87689 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
83 ✗ throw_pretty(
84 "Invalid argument: " << "x has wrong dimension (it should be " +
85 std::to_string(state_->get_nx()) + ")");
86 }
87
88
2/4
✓ Branch 4 taken 87689 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 87689 times.
✗ Branch 8 not taken.
 87689 state_->diff(xref_, x, data->r);
89 87689 }
90
91 template <typename Scalar>
92 14506 void ResidualModelStateTpl<Scalar>::calcDiff(
93 const boost::shared_ptr<ResidualDataAbstract>& data,
94 const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>&) {
95
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 14506 times.
 14506 if (static_cast<std::size_t>(x.size()) != state_->get_nx()) {
96 ✗ throw_pretty(
97 "Invalid argument: " << "x has wrong dimension (it should be " +
98 std::to_string(state_->get_nx()) + ")");
99 }
100
101
3/6
✓ Branch 5 taken 14506 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14506 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14506 times.
✗ Branch 12 not taken.
 14506 state_->Jdiff(xref_, x, data->Rx, data->Rx, second);
102 14506 }
103
104 template <typename Scalar>
105 14426 void ResidualModelStateTpl<Scalar>::calcCostDiff(
106 const boost::shared_ptr<CostDataAbstract>& cdata,
107 const boost::shared_ptr<ResidualDataAbstract>& rdata,
108 const boost::shared_ptr<ActivationDataAbstract>& adata, const bool) {
109 14426 const std::size_t nv = state_->get_nv();
110
1/2
✓ Branch 1 taken 14426 times.
✗ Branch 2 not taken.
 14426 if (pin_model_) {
111 typedef Eigen::Block<MatrixXs> MatrixBlock;
112 14426 for (pinocchio::JointIndex i = 1;
113
2/2
✓ Branch 1 taken 181752 times.
✓ Branch 2 taken 14426 times.
 196178 i < (pinocchio::JointIndex)pin_model_->njoints; ++i) {
114 181752 const MatrixBlock& RxBlock =
115
1/2
✓ Branch 6 taken 181752 times.
✗ Branch 7 not taken.
 363504 rdata->Rx.block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
116 181752 pin_model_->nvs[i], pin_model_->nvs[i]);
117
3/6
✓ Branch 6 taken 181752 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 181752 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 181752 times.
✗ Branch 13 not taken.
 181752 cdata->Lx.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]).noalias() =
118
2/4
✓ Branch 1 taken 181752 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181752 times.
✗ Branch 5 not taken.
 181752 RxBlock.transpose() *
119
1/2
✓ Branch 6 taken 181752 times.
✗ Branch 7 not taken.
 181752 adata->Ar.segment(pin_model_->idx_vs[i], pin_model_->nvs[i]);
120 181752 cdata->Lxx
121
1/2
✓ Branch 5 taken 181752 times.
✗ Branch 6 not taken.
 181752 .block(pin_model_->idx_vs[i], pin_model_->idx_vs[i],
122 181752 pin_model_->nvs[i], pin_model_->nvs[i])
123
4/8
✓ Branch 1 taken 181752 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181752 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 181752 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 181752 times.
✗ Branch 11 not taken.
 363504 .noalias() = RxBlock.transpose() *
124 181752 adata->Arr.diagonal()
125
1/2
✓ Branch 5 taken 181752 times.
✗ Branch 6 not taken.
 181752 .segment(pin_model_->idx_vs[i], pin_model_->nvs[i])
126
2/4
✓ Branch 1 taken 181752 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 181752 times.
✗ Branch 5 not taken.
 545256 .asDiagonal() *
127 RxBlock;
128 }
129
2/4
✓ Branch 4 taken 14426 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14426 times.
✗ Branch 8 not taken.
 14426 cdata->Lx.tail(nv) = adata->Ar.tail(nv);
130
3/6
✓ Branch 5 taken 14426 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 14426 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 14426 times.
✗ Branch 12 not taken.
 14426 cdata->Lxx.diagonal().tail(nv) = adata->Arr.diagonal().tail(nv);
131 } else {
132 ✗ cdata->Lx = adata->Ar;
133 ✗ cdata->Lxx.diagonal() = adata->Arr.diagonal();
134 }
135 14426 }
136
137 template <typename Scalar>
138 63 void ResidualModelStateTpl<Scalar>::print(std::ostream& os) const {
139 63 os << "ResidualModelState";
140 63 }
141
142 template <typename Scalar>
143 const typename MathBaseTpl<Scalar>::VectorXs&
144 1 ResidualModelStateTpl<Scalar>::get_reference() const {
145 1 return xref_;
146 }
147
148 template <typename Scalar>
149 1 void ResidualModelStateTpl<Scalar>::set_reference(const VectorXs& reference) {
150
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 if (static_cast<std::size_t>(reference.size()) != state_->get_nx()) {
151 ✗ throw_pretty(
152 "Invalid argument: "
153 << "the state reference has wrong dimension (" << reference.size()
154 << " provided - it should be " + std::to_string(state_->get_nx()) + ")")
155 }
156 1 xref_ = reference;
157 1 }
158
159 } // namespace crocoddyl
160