GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/model.hxx
Date: 2025-02-12 21:03:38
Exec Total Coverage
Lines: 242 264 91.7%
Branches: 284 691 41.1%
Line Branch Exec Source
1 //
2 // Copyright (c) 2015-2021 CNRS INRIA
3 // Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
4 //
5
6 #ifndef __pinocchio_multibody_model_hxx__
7 #define __pinocchio_multibody_model_hxx__
8
9 #include "pinocchio/utils/string-generator.hpp"
10 #include "pinocchio/multibody/liegroup/liegroup-algo.hpp"
11
12 /// @cond DEV
13
14 namespace pinocchio
15 {
16 namespace details
17 {
18 struct FilterFrame
19 {
20 const std::string & name;
21 const FrameType & typeMask;
22
23 112685 FilterFrame(const std::string & name, const FrameType & typeMask)
24 112685 : name(name)
25 112685 , typeMask(typeMask)
26 {
27 112685 }
28
29 template<typename Scalar, int Options>
30 3523428 bool operator()(const FrameTpl<Scalar, Options> & frame) const
31 {
32
4/4
✓ Branch 0 taken 1728522 times.
✓ Branch 1 taken 1762578 times.
✓ Branch 3 taken 42648 times.
✓ Branch 4 taken 1685874 times.
 3523428 return (typeMask & frame.type) && (name == frame.name);
33 }
34 };
35 } // namespace details
36
37 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
38 const typename ModelTpl<Scalar, Options, JointCollectionTpl>::Vector3
39 ModelTpl<Scalar, Options, JointCollectionTpl>::gravity981((Scalar)0, (Scalar)0, (Scalar)-9.81);
40
41 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
42 inline std::ostream &
43 6 operator<<(std::ostream & os, const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
44 {
45 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::Index Index;
46
47 6 os << "Nb joints = " << model.njoints << " (nq=" << model.nq << ",nv=" << model.nv << ")"
48 6 << std::endl;
49
2/2
✓ Branch 0 taken 47 times.
✓ Branch 1 taken 6 times.
 53 for (Index i = 0; i < (Index)(model.njoints); ++i)
50 {
51 47 os << " Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i]
52 47 << std::endl;
53 }
54
55 6 return os;
56 }
57
58 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
59 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
60 14297 ModelTpl<Scalar, Options, JointCollectionTpl>::addJoint(
61 const JointIndex parent,
62 const JointModel & joint_model,
63 const SE3 & joint_placement,
64 const std::string & joint_name,
65 const VectorXs & max_effort,
66 const VectorXs & max_velocity,
67 const VectorXs & min_config,
68 const VectorXs & max_config,
69 const VectorXs & joint_friction,
70 const VectorXs & joint_damping)
71 {
72
4/8
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14297 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 14297 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 14297 times.
✗ Branch 11 not taken.
 14297 assert(
73 (njoints == (int)joints.size()) && (njoints == (int)inertias.size())
74 && (njoints == (int)parents.size()) && (njoints == (int)jointPlacements.size()));
75
4/8
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 14297 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 14297 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 14297 times.
✗ Branch 9 not taken.
 14297 assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0));
76
3/6
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14297 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 14297 times.
 14297 assert(joint_model.nq() >= joint_model.nv());
77
78
2/28
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 14297 PINOCCHIO_CHECK_ARGUMENT_SIZE(
79 max_effort.size(), joint_model.nv(), "The joint maximum effort vector is not of right size");
80
2/28
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 14297 PINOCCHIO_CHECK_ARGUMENT_SIZE(
81 max_velocity.size(), joint_model.nv(),
82 "The joint maximum velocity vector is not of right size");
83
2/28
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 14297 PINOCCHIO_CHECK_ARGUMENT_SIZE(
84 min_config.size(), joint_model.nq(),
85 "The joint lower configuration bound is not of right size");
86
2/28
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 14297 PINOCCHIO_CHECK_ARGUMENT_SIZE(
87 max_config.size(), joint_model.nq(),
88 "The joint upper configuration bound is not of right size");
89
2/28
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 14297 PINOCCHIO_CHECK_ARGUMENT_SIZE(
90 joint_friction.size(), joint_model.nv(), "The joint friction vector is not of right size");
91
2/28
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 7 not taken.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
✗ Branch 13 not taken.
✗ Branch 14 not taken.
✗ Branch 16 not taken.
✗ Branch 17 not taken.
✗ Branch 19 not taken.
✗ Branch 20 not taken.
✗ Branch 23 not taken.
✗ Branch 24 not taken.
✗ Branch 26 not taken.
✗ Branch 27 not taken.
✗ Branch 29 not taken.
✗ Branch 30 not taken.
✗ Branch 32 not taken.
✗ Branch 33 not taken.
✗ Branch 35 not taken.
✗ Branch 36 not taken.
✗ Branch 39 not taken.
✗ Branch 40 not taken.
✗ Branch 42 not taken.
✗ Branch 43 not taken.
 14297 PINOCCHIO_CHECK_ARGUMENT_SIZE(
92 joint_damping.size(), joint_model.nv(), "The joint damping vector is not of right size");
93
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 14297 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 14297 PINOCCHIO_CHECK_INPUT_ARGUMENT(
94 parent < (JointIndex)njoints, "The index of the parent joint is not valid.");
95
96 14297 JointIndex joint_id = (JointIndex)(njoints++);
97
98
2/4
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 6 not taken.
 14297 joints.push_back(JointModel(joint_model.derived()));
99 14297 JointModel & jmodel = joints.back();
100
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 jmodel.setIndexes(joint_id, nq, nv);
101
102
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 const int joint_nq = jmodel.nq();
103
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 const int joint_idx_q = jmodel.idx_q();
104
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 const int joint_nv = jmodel.nv();
105
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 const int joint_idx_v = jmodel.idx_v();
106
107
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14297 times.
 14297 assert(joint_idx_q >= 0);
108
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 14297 times.
 14297 assert(joint_idx_v >= 0);
109
110
2/4
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14297 times.
✗ Branch 5 not taken.
 14297 inertias.push_back(Inertia::Zero());
111
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 parents.push_back(parent);
112
1/2
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
 14297 children.push_back(IndexVector());
113
1/2
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
 14297 children[parent].push_back(joint_id);
114
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 jointPlacements.push_back(joint_placement);
115
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 names.push_back(joint_name);
116
117 14297 nq += joint_nq;
118
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 nqs.push_back(joint_nq);
119
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 idx_qs.push_back(joint_idx_q);
120 14297 nv += joint_nv;
121
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 nvs.push_back(joint_nv);
122
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 idx_vs.push_back(joint_idx_v);
123
124
3/4
✓ Branch 0 taken 14288 times.
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 14288 times.
✗ Branch 3 not taken.
 14297 if (joint_nq > 0 && joint_nv > 0)
125 {
126
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 effortLimit.conservativeResize(nv);
127
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(effortLimit) = max_effort;
128
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 velocityLimit.conservativeResize(nv);
129
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(velocityLimit) = max_velocity;
130
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 lowerPositionLimit.conservativeResize(nq);
131
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
132
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 upperPositionLimit.conservativeResize(nq);
133
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointConfigSelector(upperPositionLimit) = max_config;
134
135
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 armature.conservativeResize(nv);
136
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(armature).setZero();
137
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 rotorInertia.conservativeResize(nv);
138
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(rotorInertia).setZero();
139
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 rotorGearRatio.conservativeResize(nv);
140
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(rotorGearRatio).setOnes();
141
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 friction.conservativeResize(nv);
142
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(friction) = joint_friction;
143
1/2
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
 14288 damping.conservativeResize(nv);
144
2/4
✓ Branch 1 taken 14288 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 14288 times.
✗ Branch 5 not taken.
 14288 jmodel.jointVelocitySelector(damping) = joint_damping;
145 }
146
147 // Init and add joint index to its parent subtrees.
148
2/4
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 14297 times.
✗ Branch 6 not taken.
 14297 subtrees.push_back(IndexVector(1));
149 14297 subtrees[joint_id][0] = joint_id;
150
1/2
✓ Branch 1 taken 14297 times.
✗ Branch 2 not taken.
 14297 addJointIndexToParentSubtrees(joint_id);
151
152 // Init and add joint index to the supports
153
1/2
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
 14297 supports.push_back(supports[parent]);
154
1/2
✓ Branch 2 taken 14297 times.
✗ Branch 3 not taken.
 14297 supports[joint_id].push_back(joint_id);
155
156 14297 return joint_id;
157 }
158
159 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
160 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
161 11485 ModelTpl<Scalar, Options, JointCollectionTpl>::addJoint(
162 const JointIndex parent,
163 const JointModel & joint_model,
164 const SE3 & joint_placement,
165 const std::string & joint_name,
166 const VectorXs & max_effort,
167 const VectorXs & max_velocity,
168 const VectorXs & min_config,
169 const VectorXs & max_config)
170 {
171
3/8
✓ Branch 1 taken 11485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11485 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 11485 const VectorXs friction = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
172
3/8
✓ Branch 1 taken 11485 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 11485 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 11485 times.
✗ Branch 8 not taken.
✗ Branch 10 not taken.
✗ Branch 11 not taken.
 11485 const VectorXs damping = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
173
174
1/2
✓ Branch 1 taken 11485 times.
✗ Branch 2 not taken.
 11485 return addJoint(
175 parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config,
176 22970 max_config, friction, damping);
177 11485 }
178
179 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
180 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
181 276 ModelTpl<Scalar, Options, JointCollectionTpl>::addJoint(
182 const JointIndex parent,
183 const JointModel & joint_model,
184 const SE3 & joint_placement,
185 const std::string & joint_name)
186 {
187
1/3
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 276 const VectorXs max_effort =
188
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 276 times.
✗ Branch 6 not taken.
 276 VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
189
1/3
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 276 const VectorXs max_velocity =
190
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 276 times.
✗ Branch 6 not taken.
 276 VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
191
1/5
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 276 const VectorXs min_config =
192
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 276 times.
✗ Branch 6 not taken.
 276 VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
193
1/3
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
✗ Branch 3 not taken.
 276 const VectorXs max_config =
194
2/6
✗ Branch 1 not taken.
✓ Branch 2 taken 276 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 276 times.
✗ Branch 6 not taken.
 276 VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());
195
196
1/2
✓ Branch 1 taken 276 times.
✗ Branch 2 not taken.
 276 return addJoint(
197 parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config,
198 552 max_config);
199 276 }
200
201 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
202 13445 FrameIndex ModelTpl<Scalar, Options, JointCollectionTpl>::addJointFrame(
203 const JointIndex & joint_index, int previous_frame_index)
204 {
205
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 13445 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 13445 PINOCCHIO_CHECK_INPUT_ARGUMENT(
206 joint_index < joints.size(),
207 "The joint index is larger than the number of joints in the model.");
208
2/2
✓ Branch 0 taken 11113 times.
✓ Branch 1 taken 2332 times.
 13445 if (previous_frame_index < 0)
209 {
210 // FIXED_JOINT is required because the parent can be the universe and its
211 // type is FIXED_JOINT
212 11113 previous_frame_index =
213
1/2
✓ Branch 3 taken 11113 times.
✗ Branch 4 not taken.
 11113 (int)getFrameId(names[parents[joint_index]], (FrameType)(JOINT | FIXED_JOINT));
214 }
215
1/2
✓ Branch 1 taken 13445 times.
✗ Branch 2 not taken.
 13445 assert((size_t)previous_frame_index < frames.size() && "Frame index out of bound");
216
217 // Add a the joint frame attached to itself to the frame vector - redundant information but
218 // useful.
219
3/6
✓ Branch 2 taken 13445 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 13445 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 13445 times.
✗ Branch 10 not taken.
 26890 return addFrame(Frame(
220 26890 names[joint_index], joint_index, (FrameIndex)previous_frame_index, SE3::Identity(), JOINT));
221 }
222
223 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
224 template<typename NewScalar>
225 typename CastType<NewScalar, ModelTpl<Scalar, Options, JointCollectionTpl>>::type
226 39 ModelTpl<Scalar, Options, JointCollectionTpl>::cast() const
227 {
228 typedef ModelTpl<NewScalar, Options, JointCollectionTpl> ReturnType;
229
230
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 ReturnType res;
231 39 res.nq = nq;
232 39 res.nv = nv;
233 39 res.njoints = njoints;
234 39 res.nbodies = nbodies;
235 39 res.nframes = nframes;
236
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.parents = parents;
237
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.children = children;
238
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.names = names;
239
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.subtrees = subtrees;
240
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.supports = supports;
241
2/4
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 26 times.
✗ Branch 5 not taken.
 39 res.gravity = gravity.template cast<NewScalar>();
242
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.name = name;
243
244
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.idx_qs = idx_qs;
245
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.nqs = nqs;
246
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.idx_vs = idx_vs;
247
1/2
✓ Branch 1 taken 26 times.
✗ Branch 2 not taken.
 39 res.nvs = nvs;
248
249 // Eigen Vectors
250
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.armature = armature.template cast<NewScalar>();
251
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.friction = friction.template cast<NewScalar>();
252
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.damping = damping.template cast<NewScalar>();
253
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.rotorInertia = rotorInertia.template cast<NewScalar>();
254
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
255
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.effortLimit = effortLimit.template cast<NewScalar>();
256
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.velocityLimit = velocityLimit.template cast<NewScalar>();
257
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
258
3/5
✓ Branch 1 taken 23 times.
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 23 times.
✗ Branch 5 not taken.
 39 res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
259
260 39 typename ConfigVectorMap::const_iterator it;
261
1/2
✗ Branch 3 not taken.
✓ Branch 4 taken 26 times.
 39 for (it = referenceConfigurations.begin(); it != referenceConfigurations.end(); it++)
262 {
263 ✗ res.referenceConfigurations.insert(
264 ✗ std::make_pair(it->first, it->second.template cast<NewScalar>()));
265 }
266
267 // reserve vectors
268
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 39 res.inertias.resize(inertias.size());
269
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 39 res.jointPlacements.resize(jointPlacements.size());
270
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 39 res.joints.resize(joints.size());
271
272 // copy into vectors
273
2/2
✓ Branch 1 taken 626 times.
✓ Branch 2 taken 26 times.
 978 for (size_t k = 0; k < joints.size(); ++k)
274 {
275
2/4
✓ Branch 2 taken 626 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 626 times.
✗ Branch 7 not taken.
 939 res.inertias[k] = inertias[k].template cast<NewScalar>();
276
2/4
✓ Branch 2 taken 626 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 626 times.
✗ Branch 7 not taken.
 939 res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
277
2/4
✓ Branch 2 taken 626 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 626 times.
✗ Branch 7 not taken.
 939 res.joints[k] = joints[k].template cast<NewScalar>();
278 }
279
280
1/2
✓ Branch 2 taken 26 times.
✗ Branch 3 not taken.
 39 res.frames.resize(frames.size());
281
2/2
✓ Branch 1 taken 1216 times.
✓ Branch 2 taken 26 times.
 1864 for (size_t k = 0; k < frames.size(); ++k)
282 {
283
2/4
✓ Branch 2 taken 1216 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 1216 times.
✗ Branch 7 not taken.
 1825 res.frames[k] = frames[k].template cast<NewScalar>();
284 }
285
286 78 return res;
287 }
288
289 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
290 92 bool ModelTpl<Scalar, Options, JointCollectionTpl>::operator==(const ModelTpl & other) const
291 {
292
2/4
✓ Branch 0 taken 54 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
 88 bool res = other.nq == nq && other.nv == nv && other.njoints == njoints
293
4/8
✓ Branch 0 taken 54 times.
✗ Branch 1 not taken.
✓ Branch 2 taken 54 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 54 times.
✗ Branch 8 not taken.
 88 && other.nbodies == nbodies && other.nframes == nframes && other.parents == parents
294
6/12
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 54 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 54 times.
✗ Branch 14 not taken.
 88 && other.children == children && other.names == names && other.subtrees == subtrees
295
5/8
✓ Branch 0 taken 54 times.
✓ Branch 1 taken 3 times.
✓ Branch 3 taken 54 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 54 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
 180 && other.gravity == gravity && other.name == name;
296
297
9/16
✓ Branch 1 taken 57 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
✓ Branch 4 taken 3 times.
✓ Branch 6 taken 54 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 54 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 54 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 54 times.
✗ Branch 14 not taken.
✓ Branch 16 taken 54 times.
✗ Branch 17 not taken.
✓ Branch 18 taken 54 times.
✗ Branch 19 not taken.
 92 res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs;
298
299
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 57 times.
 92 if (other.referenceConfigurations.size() != referenceConfigurations.size())
300 ✗ return false;
301
302 92 typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
303 92 typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
304
2/2
✓ Branch 1 taken 22 times.
✓ Branch 2 taken 56 times.
 134 for (long k = 0; k < (long)referenceConfigurations.size(); ++k)
305 {
306
2/2
✓ Branch 4 taken 1 times.
✓ Branch 5 taken 21 times.
 44 if (it->second.size() != it_other->second.size())
307 2 return false;
308
2/4
✓ Branch 3 taken 21 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
✓ Branch 6 taken 21 times.
 42 if (it->second != it_other->second)
309 ✗ return false;
310
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 std::advance(it, 1);
311
1/2
✓ Branch 1 taken 21 times.
✗ Branch 2 not taken.
 42 std::advance(it_other, 1);
312 }
313
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 56 times.
 90 if (other.armature.size() != armature.size())
314 ✗ return false;
315
1/2
✓ Branch 1 taken 56 times.
✗ Branch 2 not taken.
 90 res &= other.armature == armature;
316
2/2
✓ Branch 0 taken 2 times.
✓ Branch 1 taken 54 times.
 90 if (!res)
317 2 return res;
318
319
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
 88 if (other.friction.size() != friction.size())
320 ✗ return false;
321
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 88 res &= other.friction == friction;
322
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 54 times.
 88 if (!res)
323 ✗ return res;
324
325
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
 88 if (other.damping.size() != damping.size())
326 ✗ return false;
327
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 88 res &= other.damping == damping;
328
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 54 times.
 88 if (!res)
329 ✗ return res;
330
331
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
 88 if (other.rotorInertia.size() != rotorInertia.size())
332 ✗ return false;
333
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 88 res &= other.rotorInertia == rotorInertia;
334
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 54 times.
 88 if (!res)
335 ✗ return res;
336
337
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
 88 if (other.rotorGearRatio.size() != rotorGearRatio.size())
338 ✗ return false;
339
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 88 res &= other.rotorGearRatio == rotorGearRatio;
340
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 54 times.
 88 if (!res)
341 ✗ return res;
342
343
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 54 times.
 88 if (other.effortLimit.size() != effortLimit.size())
344 ✗ return false;
345
1/2
✓ Branch 1 taken 54 times.
✗ Branch 2 not taken.
 88 res &= other.effortLimit == effortLimit;
346
2/2
✓ Branch 0 taken 1 times.
✓ Branch 1 taken 53 times.
 88 if (!res)
347 1 return res;
348
349
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 53 times.
 87 if (other.velocityLimit.size() != velocityLimit.size())
350 ✗ return false;
351
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 87 res &= other.velocityLimit == velocityLimit;
352
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 53 times.
 87 if (!res)
353 ✗ return res;
354
355
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 53 times.
 87 if (other.lowerPositionLimit.size() != lowerPositionLimit.size())
356 ✗ return false;
357
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 87 res &= other.lowerPositionLimit == lowerPositionLimit;
358
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 53 times.
 87 if (!res)
359 ✗ return res;
360
361
1/2
✗ Branch 2 not taken.
✓ Branch 3 taken 53 times.
 87 if (other.upperPositionLimit.size() != upperPositionLimit.size())
362 ✗ return false;
363
1/2
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
 87 res &= other.upperPositionLimit == upperPositionLimit;
364
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 53 times.
 87 if (!res)
365 ✗ return res;
366
367
2/2
✓ Branch 1 taken 1651 times.
✓ Branch 2 taken 53 times.
 2750 for (size_t k = 1; k < inertias.size(); ++k)
368 {
369
1/2
✓ Branch 3 taken 1651 times.
✗ Branch 4 not taken.
 2663 res &= other.inertias[k] == inertias[k];
370
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1651 times.
 2663 if (!res)
371 ✗ return res;
372 }
373
374
2/2
✓ Branch 1 taken 1651 times.
✓ Branch 2 taken 53 times.
 2750 for (size_t k = 1; k < other.jointPlacements.size(); ++k)
375 {
376
1/2
✓ Branch 3 taken 1651 times.
✗ Branch 4 not taken.
 2663 res &= other.jointPlacements[k] == jointPlacements[k];
377
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 1651 times.
 2663 if (!res)
378 ✗ return res;
379 }
380
381
4/8
✓ Branch 1 taken 53 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 53 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 53 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 53 times.
✗ Branch 9 not taken.
 87 res &= other.joints == joints && other.frames == frames;
382
383 87 return res;
384 }
385
386 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
387 13796 void ModelTpl<Scalar, Options, JointCollectionTpl>::appendBodyToJoint(
388 const typename ModelTpl::JointIndex joint_index, const Inertia & Y, const SE3 & body_placement)
389 {
390
1/2
✓ Branch 1 taken 13796 times.
✗ Branch 2 not taken.
 13796 const Inertia & iYf = Y.se3Action(body_placement);
391
1/2
✓ Branch 2 taken 13796 times.
✗ Branch 3 not taken.
 13796 inertias[joint_index] += iYf;
392 13796 nbodies++;
393 13796 }
394
395 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
396 typename ModelTpl<Scalar, Options, JointCollectionTpl>::FrameIndex
397 15248 ModelTpl<Scalar, Options, JointCollectionTpl>::addBodyFrame(
398 const std::string & body_name,
399 const JointIndex & parentJoint,
400 const SE3 & body_placement,
401 int parentFrame)
402 {
403
2/2
✓ Branch 0 taken 11816 times.
✓ Branch 1 taken 3432 times.
 15248 if (parentFrame < 0)
404 {
405 // FIXED_JOINT is required because the parent can be the universe and its
406 // type is FIXED_JOINT
407
1/2
✓ Branch 2 taken 11816 times.
✗ Branch 3 not taken.
 11816 parentFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
408 }
409
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 15248 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 15248 PINOCCHIO_CHECK_INPUT_ARGUMENT((size_t)parentFrame < frames.size(), "Frame index out of bound");
410
2/4
✓ Branch 2 taken 15248 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 15248 times.
✗ Branch 6 not taken.
 15248 return addFrame(Frame(body_name, parentJoint, (FrameIndex)parentFrame, body_placement, BODY));
411 }
412
413 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
414 inline typename ModelTpl<Scalar, Options, JointCollectionTpl>::FrameIndex
415 3172 ModelTpl<Scalar, Options, JointCollectionTpl>::getBodyId(const std::string & name) const
416 {
417
1/2
✓ Branch 1 taken 3172 times.
✗ Branch 2 not taken.
 3172 return getFrameId(name, BODY);
418 }
419
420 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
421 inline bool
422 6294 ModelTpl<Scalar, Options, JointCollectionTpl>::existBodyName(const std::string & name) const
423 {
424
1/2
✓ Branch 1 taken 6294 times.
✗ Branch 2 not taken.
 6294 return existFrame(name, BODY);
425 }
426
427 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
428 inline typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
429 14415 ModelTpl<Scalar, Options, JointCollectionTpl>::getJointId(const std::string & name) const
430 {
431 typedef std::vector<std::string>::iterator::difference_type it_diff_t;
432
1/2
✓ Branch 4 taken 14415 times.
✗ Branch 5 not taken.
 14415 it_diff_t res = std::find(names.begin(), names.end(), name) - names.begin();
433
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 14415 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 14415 PINOCCHIO_CHECK_INPUT_ARGUMENT(
434 (res < INT_MAX), "Id superior to int range. Should never happen.");
435 14415 return ModelTpl::JointIndex(res);
436 }
437
438 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
439 inline bool
440 1286 ModelTpl<Scalar, Options, JointCollectionTpl>::existJointName(const std::string & name) const
441 {
442 1286 return (names.end() != std::find(names.begin(), names.end(), name));
443 }
444
445 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
446 inline typename ModelTpl<Scalar, Options, JointCollectionTpl>::FrameIndex
447 34555 ModelTpl<Scalar, Options, JointCollectionTpl>::getFrameId(
448 const std::string & name, const FrameType & type) const
449 {
450 typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it =
451
1/2
✓ Branch 4 taken 34191 times.
✗ Branch 5 not taken.
 34555 std::find_if(frames.begin(), frames.end(), details::FilterFrame(name, type));
452
8/10
✓ Branch 2 taken 34190 times.
✓ Branch 3 taken 1 times.
✓ Branch 9 taken 34190 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 1 times.
✓ Branch 13 taken 34189 times.
✓ Branch 14 taken 1 times.
✓ Branch 15 taken 34190 times.
✓ Branch 18 taken 1 times.
✗ Branch 19 not taken.
 34555 PINOCCHIO_CHECK_INPUT_ARGUMENT(
453 ((it == frames.end()
454 || (std::find_if(boost::next(it), frames.end(), details::FilterFrame(name, type)) == frames.end()))),
455 "Several frames match the filter - please specify the FrameType");
456 34554 return FrameIndex(it - frames.begin());
457 }
458
459 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
460 44473 inline bool ModelTpl<Scalar, Options, JointCollectionTpl>::existFrame(
461 const std::string & name, const FrameType & type) const
462 {
463
1/2
✓ Branch 4 taken 44304 times.
✗ Branch 5 not taken.
 44473 return std::find_if(frames.begin(), frames.end(), details::FilterFrame(name, type))
464 88946 != frames.end();
465 }
466
467 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
468 typename ModelTpl<Scalar, Options, JointCollectionTpl>::FrameIndex
469 32079 ModelTpl<Scalar, Options, JointCollectionTpl>::addFrame(
470 const Frame & frame, const bool append_inertia)
471 {
472
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 32079 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 32079 PINOCCHIO_CHECK_INPUT_ARGUMENT(
473 frame.parentJoint < (JointIndex)njoints, "The index of the parent joint is not valid.");
474
475 // TODO: fix it
476 // PINOCCHIO_CHECK_INPUT_ARGUMENT(frame.inertia.isValid(),
477 // "The input inertia is not valid.")
478
479 // Check if the frame.name exists with the same type
480
2/2
✓ Branch 1 taken 190 times.
✓ Branch 2 taken 31889 times.
 32079 if (existFrame(frame.name, frame.type))
481 {
482 190 return getFrameId(frame.name, frame.type);
483 }
484 // else: we must add a new frames to the current stack
485 31889 frames.push_back(frame);
486
2/2
✓ Branch 0 taken 31169 times.
✓ Branch 1 taken 720 times.
 31889 if (append_inertia)
487
1/2
✓ Branch 3 taken 31169 times.
✗ Branch 4 not taken.
 31169 inertias[frame.parentJoint] += frame.placement.act(frame.inertia);
488 31889 nframes++;
489 31889 return FrameIndex(nframes - 1);
490 }
491
492 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
493 14297 void ModelTpl<Scalar, Options, JointCollectionTpl>::addJointIndexToParentSubtrees(
494 const JointIndex joint_id)
495 {
496
2/2
✓ Branch 2 taken 60068 times.
✓ Branch 3 taken 14297 times.
 74365 for (JointIndex parent = parents[joint_id]; parent > 0; parent = parents[parent])
497 60068 subtrees[parent].push_back(joint_id);
498
499 // Also add joint_id to the universe
500 14297 subtrees[0].push_back(joint_id);
501 14297 }
502
503 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
504 1 std::vector<bool> ModelTpl<Scalar, Options, JointCollectionTpl>::hasConfigurationLimit()
505 {
506 1 std::vector<bool> vec;
507
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (Index i = 1; i < (Index)(njoints); ++i)
508 {
509
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::vector<bool> & cf_limits = joints[i].hasConfigurationLimit();
510
1/2
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 vec.insert(vec.end(), cf_limits.begin(), cf_limits.end());
511 }
512 1 return vec;
513 }
514
515 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
516 1 std::vector<bool> ModelTpl<Scalar, Options, JointCollectionTpl>::hasConfigurationLimitInTangent()
517 {
518 1 std::vector<bool> vec;
519
2/2
✓ Branch 0 taken 3 times.
✓ Branch 1 taken 1 times.
 4 for (Index i = 1; i < (Index)(njoints); ++i)
520 {
521
1/2
✓ Branch 2 taken 3 times.
✗ Branch 3 not taken.
 3 const std::vector<bool> & cf_limits = joints[i].hasConfigurationLimitInTangent();
522
1/2
✓ Branch 5 taken 3 times.
✗ Branch 6 not taken.
 3 vec.insert(vec.end(), cf_limits.begin(), cf_limits.end());
523 }
524 1 return vec;
525 }
526
527 } // namespace pinocchio
528
529 /// @endcond
530
531 #endif // ifndef __pinocchio_multibody_model_hxx__
532