GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/multibody/impulses/multiple-impulses.hxx
Date: 2025-03-26 19:23:43
Exec Total Coverage
Lines: 177 208 85.1%
Branches: 137 495 27.7%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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/impulses/multiple-impulses.hpp"
11
12 namespace crocoddyl {
13
14 template <typename Scalar>
15 103 ImpulseModelMultipleTpl<Scalar>::ImpulseModelMultipleTpl(
16 std::shared_ptr<StateMultibody> state)
17 103 : state_(state), nc_(0), nc_total_(0) {}
18
19 template <typename Scalar>
20 105 ImpulseModelMultipleTpl<Scalar>::~ImpulseModelMultipleTpl() {}
21
22 template <typename Scalar>
23 266 void ImpulseModelMultipleTpl<Scalar>::addImpulse(
24 const std::string& name, std::shared_ptr<ImpulseModelAbstract> impulse,
25 const bool active) {
26 std::pair<typename ImpulseModelContainer::iterator, bool> ret =
27
3/6
✓ Branch 1 taken 266 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 266 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 266 times.
✗ Branch 8 not taken.
 266 impulses_.insert(std::make_pair(
28 name, std::make_shared<ImpulseItem>(name, impulse, active)));
29
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 265 times.
 266 if (ret.second == false) {
30 std::cerr << "Warning: we couldn't add the " << name
31
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 << " impulse item, it already existed." << std::endl;
32
2/2
✓ Branch 0 taken 264 times.
✓ Branch 1 taken 1 times.
 265 } else if (active) {
33
1/2
✓ Branch 2 taken 264 times.
✗ Branch 3 not taken.
 264 nc_ += impulse->get_nc();
34
1/2
✓ Branch 2 taken 264 times.
✗ Branch 3 not taken.
 264 nc_total_ += impulse->get_nc();
35
1/2
✓ Branch 1 taken 264 times.
✗ Branch 2 not taken.
 264 active_set_.insert(name);
36
1/2
✓ Branch 0 taken 1 times.
✗ Branch 1 not taken.
 1 } else if (!active) {
37
1/2
✓ Branch 2 taken 1 times.
✗ Branch 3 not taken.
 1 nc_total_ += impulse->get_nc();
38
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inactive_set_.insert(name);
39 }
40 266 }
41
42 template <typename Scalar>
43 2 void ImpulseModelMultipleTpl<Scalar>::removeImpulse(const std::string& name) {
44
1/2
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
 2 typename ImpulseModelContainer::iterator it = impulses_.find(name);
45
2/2
✓ Branch 2 taken 1 times.
✓ Branch 3 taken 1 times.
 2 if (it != impulses_.end()) {
46
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 nc_ -= it->second->impulse->get_nc();
47
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 nc_total_ -= it->second->impulse->get_nc();
48
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 impulses_.erase(it);
49
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 active_set_.erase(name);
50
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inactive_set_.erase(name);
51 } else {
52 std::cerr << "Warning: we couldn't remove the " << name
53
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 << " impulse item, it doesn't exist." << std::endl;
54 }
55 2 }
56
57 template <typename Scalar>
58 9 void ImpulseModelMultipleTpl<Scalar>::changeImpulseStatus(
59 const std::string& name, const bool active) {
60
1/2
✓ Branch 1 taken 9 times.
✗ Branch 2 not taken.
 9 typename ImpulseModelContainer::iterator it = impulses_.find(name);
61
2/2
✓ Branch 2 taken 8 times.
✓ Branch 3 taken 1 times.
 9 if (it != impulses_.end()) {
62
5/6
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 7 times.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 1 times.
✓ Branch 7 taken 7 times.
 8 if (active && !it->second->active) {
63
1/2
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
 1 nc_ += it->second->impulse->get_nc();
64
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 active_set_.insert(name);
65
1/2
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
 1 inactive_set_.erase(name);
66
3/6
✓ Branch 0 taken 7 times.
✗ Branch 1 not taken.
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 7 times.
✗ Branch 7 not taken.
 7 } else if (!active && it->second->active) {
67
1/2
✓ Branch 4 taken 7 times.
✗ Branch 5 not taken.
 7 nc_ -= it->second->impulse->get_nc();
68
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 inactive_set_.insert(name);
69
1/2
✓ Branch 1 taken 7 times.
✗ Branch 2 not taken.
 7 active_set_.erase(name);
70 }
71 8 it->second->active = active;
72 } else {
73 std::cerr << "Warning: we couldn't change the status of the " << name
74
4/8
✓ Branch 1 taken 1 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 1 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 1 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 1 times.
✗ Branch 11 not taken.
 1 << " impulse item, it doesn't exist." << std::endl;
75 }
76 9 }
77
78 template <typename Scalar>
79 6988 void ImpulseModelMultipleTpl<Scalar>::calc(
80 const std::shared_ptr<ImpulseDataMultiple>& data,
81 const Eigen::Ref<const VectorXs>& x) {
82
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 6988 times.
 6988 if (data->impulses.size() != impulses_.size()) {
83 ✗ throw_pretty("Invalid argument: "
84 << "it doesn't match the number of impulse datas and models");
85 }
86
87 6988 std::size_t nc = 0;
88
1/2
✓ Branch 2 taken 6988 times.
✗ Branch 3 not taken.
 6988 const std::size_t nv = state_->get_nv();
89 6988 typename ImpulseModelContainer::iterator it_m, end_m;
90 6988 typename ImpulseDataContainer::iterator it_d, end_d;
91 13976 for (it_m = impulses_.begin(), end_m = impulses_.end(),
92 6988 it_d = data->impulses.begin(), end_d = data->impulses.end();
93
5/6
✓ Branch 3 taken 6988 times.
✓ Branch 4 taken 16476 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6988 times.
✓ Branch 8 taken 16476 times.
✓ Branch 9 taken 6988 times.
 23464 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
94 16476 const std::shared_ptr<ImpulseItem>& m_i = it_m->second;
95
2/2
✓ Branch 1 taken 16472 times.
✓ Branch 2 taken 4 times.
 16476 if (m_i->active) {
96 16472 const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
97
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 16472 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 16472 assert_pretty(it_m->first == it_d->first,
98 "it doesn't match the impulse name between model and data ("
99 << it_m->first << " != " << it_d->first << ")");
100
101
1/2
✓ Branch 3 taken 16472 times.
✗ Branch 4 not taken.
 16472 m_i->impulse->calc(d_i, x);
102
1/2
✓ Branch 3 taken 16472 times.
✗ Branch 4 not taken.
 16472 const std::size_t nc_i = m_i->impulse->get_nc();
103
2/4
✓ Branch 3 taken 16472 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16472 times.
✗ Branch 7 not taken.
 16472 data->Jc.block(nc, 0, nc_i, nv) = d_i->Jc;
104 16472 nc += nc_i;
105 }
106 }
107 6988 }
108
109 template <typename Scalar>
110 254 void ImpulseModelMultipleTpl<Scalar>::calcDiff(
111 const std::shared_ptr<ImpulseDataMultiple>& data,
112 const Eigen::Ref<const VectorXs>& x) {
113
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 254 times.
 254 if (data->impulses.size() != impulses_.size()) {
114 ✗ throw_pretty("Invalid argument: "
115 << "it doesn't match the number of impulse datas and models");
116 }
117
118 254 std::size_t nc = 0;
119
1/2
✓ Branch 2 taken 254 times.
✗ Branch 3 not taken.
 254 const std::size_t nv = state_->get_nv();
120 254 typename ImpulseModelContainer::iterator it_m, end_m;
121 254 typename ImpulseDataContainer::iterator it_d, end_d;
122 508 for (it_m = impulses_.begin(), end_m = impulses_.end(),
123 254 it_d = data->impulses.begin(), end_d = data->impulses.end();
124
5/6
✓ Branch 3 taken 254 times.
✓ Branch 4 taken 724 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 254 times.
✓ Branch 8 taken 724 times.
✓ Branch 9 taken 254 times.
 978 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
125 724 const std::shared_ptr<ImpulseItem>& m_i = it_m->second;
126
2/2
✓ Branch 1 taken 720 times.
✓ Branch 2 taken 4 times.
 724 if (m_i->active) {
127 720 const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
128
1/18
✗ Branch 3 not taken.
✓ Branch 4 taken 720 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 20 not taken.
✗ Branch 21 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 27 not taken.
✗ Branch 28 not taken.
✗ Branch 30 not taken.
✗ Branch 31 not taken.
 720 assert_pretty(it_m->first == it_d->first,
129 "it doesn't match the impulse name between model and data ("
130 << it_m->first << " != " << it_d->first << ")");
131
132
1/2
✓ Branch 3 taken 720 times.
✗ Branch 4 not taken.
 720 m_i->impulse->calcDiff(d_i, x);
133
1/2
✓ Branch 3 taken 720 times.
✗ Branch 4 not taken.
 720 const std::size_t nc_i = m_i->impulse->get_nc();
134
2/4
✓ Branch 3 taken 720 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 720 times.
✗ Branch 7 not taken.
 720 data->dv0_dq.block(nc, 0, nc_i, nv) = d_i->dv0_dq;
135 720 nc += nc_i;
136 }
137 }
138 254 }
139
140 template <typename Scalar>
141 6980 void ImpulseModelMultipleTpl<Scalar>::updateVelocity(
142 const std::shared_ptr<ImpulseDataMultiple>& data,
143 const VectorXs& vnext) const {
144
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 6980 times.
 6980 if (static_cast<std::size_t>(vnext.size()) != state_->get_nv()) {
145 ✗ throw_pretty(
146 "Invalid argument: " << "vnext has wrong dimension (it should be " +
147 std::to_string(state_->get_nv()) + ")");
148 }
149 6980 data->vnext = vnext;
150 6980 }
151
152 template <typename Scalar>
153 6981 void ImpulseModelMultipleTpl<Scalar>::updateForce(
154 const std::shared_ptr<ImpulseDataMultiple>& data, const VectorXs& force) {
155
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 6981 times.
 6981 if (static_cast<std::size_t>(force.size()) != nc_) {
156 ✗ throw_pretty(
157 "Invalid argument: " << "force has wrong dimension (it should be " +
158 std::to_string(nc_) + ")");
159 }
160
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 6981 times.
 6981 if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
161 ✗ throw_pretty("Invalid argument: "
162 << "it doesn't match the number of impulse datas and models");
163 }
164
165
2/2
✓ Branch 5 taken 210488 times.
✓ Branch 6 taken 6981 times.
 217469 for (ForceIterator it = data->fext.begin(); it != data->fext.end(); ++it) {
166
2/4
✓ Branch 1 taken 210488 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 210488 times.
✗ Branch 6 not taken.
 210488 *it = pinocchio::ForceTpl<Scalar>::Zero();
167 }
168
169 6981 std::size_t nc = 0;
170 6981 typename ImpulseModelContainer::iterator it_m, end_m;
171 6981 typename ImpulseDataContainer::iterator it_d, end_d;
172 13962 for (it_m = impulses_.begin(), end_m = impulses_.end(),
173 6981 it_d = data->impulses.begin(), end_d = data->impulses.end();
174
5/6
✓ Branch 3 taken 6981 times.
✓ Branch 4 taken 16453 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6981 times.
✓ Branch 8 taken 16453 times.
✓ Branch 9 taken 6981 times.
 23434 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
175 16453 const std::shared_ptr<ImpulseItem>& m_i = it_m->second;
176 16453 const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
177
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 16453 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 16453 assert_pretty(it_m->first == it_d->first,
178 "it doesn't match the impulse name between data and model");
179
1/2
✓ Branch 1 taken 16453 times.
✗ Branch 2 not taken.
 16453 if (m_i->active) {
180
1/2
✓ Branch 3 taken 16453 times.
✗ Branch 4 not taken.
 16453 const std::size_t nc_i = m_i->impulse->get_nc();
181 const Eigen::VectorBlock<const VectorXs, Eigen::Dynamic> force_i =
182
1/2
✓ Branch 1 taken 16453 times.
✗ Branch 2 not taken.
 16453 force.segment(nc, nc_i);
183
2/4
✓ Branch 3 taken 16453 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 16453 times.
✗ Branch 7 not taken.
 16453 m_i->impulse->updateForce(d_i, force_i);
184 #if PINOCCHIO_VERSION_AT_LEAST(3, 0, 0)
185 16453 const pinocchio::JointIndex joint =
186
1/2
✓ Branch 2 taken 16453 times.
✗ Branch 3 not taken.
 16453 state_->get_pinocchio()->frames[d_i->frame].parentJoint;
187 #else
188 const pinocchio::JointIndex joint =
189 state_->get_pinocchio()->frames[d_i->frame].parent;
190 #endif
191
1/2
✓ Branch 4 taken 16453 times.
✗ Branch 5 not taken.
 16453 data->fext[joint] = d_i->fext;
192 16453 nc += nc_i;
193 } else {
194 ✗ m_i->impulse->setZeroForce(d_i);
195 }
196 }
197 6981 }
198
199 template <typename Scalar>
200 249 void ImpulseModelMultipleTpl<Scalar>::updateVelocityDiff(
201 const std::shared_ptr<ImpulseDataMultiple>& data,
202 const MatrixXs& dvnext_dx) const {
203
2/4
✓ Branch 3 taken 249 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 249 times.
 498 if (static_cast<std::size_t>(dvnext_dx.rows()) != state_->get_nv() ||
204
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 249 times.
 249 static_cast<std::size_t>(dvnext_dx.cols()) != state_->get_ndx()) {
205 ✗ throw_pretty(
206 "Invalid argument: " << "dvnext_dx has wrong dimension (it should be " +
207 std::to_string(state_->get_nv()) + "," +
208 std::to_string(state_->get_ndx()) + ")");
209 }
210 249 data->dvnext_dx = dvnext_dx;
211 249 }
212
213 template <typename Scalar>
214 248 void ImpulseModelMultipleTpl<Scalar>::updateForceDiff(
215 const std::shared_ptr<ImpulseDataMultiple>& data,
216 const MatrixXs& df_dx) const {
217
1/2
✓ Branch 2 taken 248 times.
✗ Branch 3 not taken.
 248 const std::size_t ndx = state_->get_ndx();
218
2/4
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✓ Branch 4 taken 248 times.
 496 if (static_cast<std::size_t>(df_dx.rows()) != nc_ ||
219
1/2
✗ Branch 1 not taken.
✓ Branch 2 taken 248 times.
 248 static_cast<std::size_t>(df_dx.cols()) != ndx) {
220 ✗ throw_pretty("Invalid argument: "
221 << "df_dx has wrong dimension (it should be " +
222 std::to_string(nc_) + "," + std::to_string(ndx) + ")");
223 }
224
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 248 times.
 248 if (static_cast<std::size_t>(data->impulses.size()) != impulses_.size()) {
225 ✗ throw_pretty("Invalid argument: "
226 << "it doesn't match the number of impulse datas and models");
227 }
228
229 248 std::size_t nc = 0;
230 248 typename ImpulseModelContainer::const_iterator it_m, end_m;
231 248 typename ImpulseDataContainer::const_iterator it_d, end_d;
232 248 for (it_m = impulses_.begin(), end_m = impulses_.end(),
233 248 it_d = data->impulses.begin(), end_d = data->impulses.end();
234
5/6
✓ Branch 3 taken 248 times.
✓ Branch 4 taken 700 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 248 times.
✓ Branch 8 taken 700 times.
✓ Branch 9 taken 248 times.
 948 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
235 700 const std::shared_ptr<ImpulseItem>& m_i = it_m->second;
236 700 const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
237
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 700 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 700 assert_pretty(it_m->first == it_d->first,
238 "it doesn't match the impulse name between data and model");
239
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 if (m_i->active) {
240
1/2
✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
 700 const std::size_t nc_i = m_i->impulse->get_nc();
241 const Eigen::Block<const MatrixXs> df_dx_i =
242
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 df_dx.block(nc, 0, nc_i, ndx);
243
2/4
✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 700 times.
✗ Branch 7 not taken.
 700 m_i->impulse->updateForceDiff(d_i, df_dx_i);
244 700 nc += nc_i;
245 } else {
246 ✗ m_i->impulse->setZeroForceDiff(d_i);
247 }
248 }
249 248 }
250
251 template <typename Scalar>
252 248 void ImpulseModelMultipleTpl<Scalar>::updateRneaDiff(
253 const std::shared_ptr<ImpulseDataMultiple>& data,
254 pinocchio::DataTpl<Scalar>& pinocchio) const {
255
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 248 times.
 496 if (static_cast<std::size_t>(data->impulses.size()) !=
256
1/2
✓ Branch 1 taken 248 times.
✗ Branch 2 not taken.
 248 this->get_impulses().size()) {
257 ✗ throw_pretty("Invalid argument: "
258 << "it doesn't match the number of impulse datas and models");
259 }
260 248 typename ImpulseModelContainer::const_iterator it_m, end_m;
261 248 typename ImpulseDataContainer::const_iterator it_d, end_d;
262 248 for (it_m = impulses_.begin(), end_m = impulses_.end(),
263 248 it_d = data->impulses.begin(), end_d = data->impulses.end();
264
5/6
✓ Branch 3 taken 248 times.
✓ Branch 4 taken 700 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 248 times.
✓ Branch 8 taken 700 times.
✓ Branch 9 taken 248 times.
 948 it_m != end_m || it_d != end_d; ++it_m, ++it_d) {
265 700 const std::shared_ptr<ImpulseItem>& m_i = it_m->second;
266 700 const std::shared_ptr<ImpulseDataAbstract>& d_i = it_d->second;
267
1/10
✗ Branch 3 not taken.
✓ Branch 4 taken 700 times.
✗ Branch 6 not taken.
✗ Branch 7 not taken.
✗ Branch 9 not taken.
✗ Branch 10 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
 700 assert_pretty(it_m->first == it_d->first,
268 "it doesn't match the impulse name between data and model");
269
1/2
✓ Branch 1 taken 700 times.
✗ Branch 2 not taken.
 700 if (m_i->active) {
270
3/5
✓ Branch 3 taken 700 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 364 times.
✓ Branch 6 taken 336 times.
✗ Branch 7 not taken.
 700 switch (m_i->impulse->get_type()) {
271 364 case pinocchio::ReferenceFrame::LOCAL:
272 364 break;
273 336 case pinocchio::ReferenceFrame::WORLD:
274 case pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED:
275
1/2
✓ Branch 2 taken 336 times.
✗ Branch 3 not taken.
 336 pinocchio.dtau_dq += d_i->dtau_dq;
276 336 break;
277 }
278 }
279 }
280 248 }
281
282 template <typename Scalar>
283 std::shared_ptr<ImpulseDataMultipleTpl<Scalar> >
284 4270 ImpulseModelMultipleTpl<Scalar>::createData(
285 pinocchio::DataTpl<Scalar>* const data) {
286 return std::allocate_shared<ImpulseDataMultiple>(
287
1/2
✓ Branch 2 taken 4270 times.
✗ Branch 3 not taken.
 8540 Eigen::aligned_allocator<ImpulseDataMultiple>(), this, data);
288 }
289
290 template <typename Scalar>
291 template <typename NewScalar>
292 2 ImpulseModelMultipleTpl<NewScalar> ImpulseModelMultipleTpl<Scalar>::cast()
293 const {
294 typedef ImpulseModelMultipleTpl<NewScalar> ReturnType;
295 typedef StateMultibodyTpl<NewScalar> StateType;
296 typedef ImpulseItemTpl<NewScalar> ImpulseType;
297
3/6
✓ Branch 1 taken 2 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 2 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 2 times.
✗ Branch 8 not taken.
 4 ReturnType ret(
298 2 std::make_shared<StateType>(state_->template cast<NewScalar>()));
299 2 typename ImpulseModelContainer::const_iterator it_m, end_m;
300
1/2
✗ Branch 4 not taken.
✓ Branch 5 taken 2 times.
 2 for (it_m = impulses_.begin(), end_m = impulses_.end(); it_m != end_m;
301 ✗ ++it_m) {
302 ✗ const std::string name = it_m->first;
303 ✗ const ImpulseType& m_i = it_m->second->template cast<NewScalar>();
304 ✗ ret.addImpulse(name, m_i.impulse, m_i.active);
305 }
306 4 return ret;
307 }
308
309 template <typename Scalar>
310 const std::shared_ptr<StateMultibodyTpl<Scalar> >&
311 25643 ImpulseModelMultipleTpl<Scalar>::get_state() const {
312 25643 return state_;
313 }
314
315 template <typename Scalar>
316 const typename ImpulseModelMultipleTpl<Scalar>::ImpulseModelContainer&
317 19023 ImpulseModelMultipleTpl<Scalar>::get_impulses() const {
318 19023 return impulses_;
319 }
320
321 template <typename Scalar>
322 7282 std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc() const {
323 7282 return nc_;
324 }
325
326 template <typename Scalar>
327 21312 std::size_t ImpulseModelMultipleTpl<Scalar>::get_nc_total() const {
328 21312 return nc_total_;
329 }
330
331 template <typename Scalar>
332 1 const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_active_set()
333 const {
334 1 return active_set_;
335 }
336
337 template <typename Scalar>
338 1 const std::set<std::string>& ImpulseModelMultipleTpl<Scalar>::get_inactive_set()
339 const {
340 1 return inactive_set_;
341 }
342
343 template <typename Scalar>
344 ✗ bool ImpulseModelMultipleTpl<Scalar>::getImpulseStatus(
345 const std::string& name) const {
346 ✗ typename ImpulseModelContainer::const_iterator it = impulses_.find(name);
347 ✗ if (it != impulses_.end()) {
348 ✗ return it->second->active;
349 } else {
350 std::cerr << "Warning: we couldn't get the status of the " << name
351 ✗ << " impulse item, it doesn't exist." << std::endl;
352 ✗ return false;
353 }
354 }
355
356 template <class Scalar>
357 1 std::ostream& operator<<(std::ostream& os,
358 const ImpulseModelMultipleTpl<Scalar>& model) {
359 1 const auto& active = model.get_active_set();
360 1 const auto& inactive = model.get_inactive_set();
361 1 os << "ImpulseModelMultiple:" << std::endl;
362 1 os << " Active:" << std::endl;
363 1 for (std::set<std::string>::const_iterator it = active.begin();
364
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 it != active.end(); ++it) {
365 const std::shared_ptr<
366 ✗ typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
367 ✗ model.get_impulses().find(*it)->second;
368 ✗ if (it != --active.end()) {
369 ✗ os << " " << *it << ": " << *impulse_item << std::endl;
370 } else {
371 ✗ os << " " << *it << ": " << *impulse_item << std::endl;
372 }
373 }
374 1 os << " Inactive:" << std::endl;
375 1 for (std::set<std::string>::const_iterator it = inactive.begin();
376
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 1 times.
 1 it != inactive.end(); ++it) {
377 const std::shared_ptr<
378 ✗ typename ImpulseModelMultipleTpl<Scalar>::ImpulseItem>& impulse_item =
379 ✗ model.get_impulses().find(*it)->second;
380 ✗ if (it != --inactive.end()) {
381 ✗ os << " " << *it << ": " << *impulse_item << std::endl;
382 } else {
383 ✗ os << " " << *it << ": " << *impulse_item;
384 }
385 }
386 1 return os;
387 }
388
389 } // namespace crocoddyl
390