GCC Code Coverage Report

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