GCC Code Coverage Report

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