GCC Code Coverage Report

Directory: ./
File: include/pinocchio/multibody/model.hxx
Date: 2024-08-27 18:20:05
Exec Total Coverage
Lines: 138 264 52.3%
Branches: 139 673 20.7%
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 33023 FilterFrame(const std::string & name, const FrameType & typeMask)
24 33023 : name(name)
25 33023 , typeMask(typeMask)
26 {
27 33023 }
28
29 template<typename Scalar, int Options>
30 753802 bool operator()(const FrameTpl<Scalar, Options> & frame) const
31 {
32
4/4
✓ Branch 0 taken 371169 times.
✓ Branch 1 taken 382633 times.
✓ Branch 3 taken 11822 times.
✓ Branch 4 taken 359347 times.
 753802 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 1 operator<<(std::ostream & os, const ModelTpl<Scalar, Options, JointCollectionTpl> & model)
44 {
45 typedef typename ModelTpl<Scalar, Options, JointCollectionTpl>::Index Index;
46
47 1 os << "Nb joints = " << model.njoints << " (nq=" << model.nq << ",nv=" << model.nv << ")"
48 1 << std::endl;
49
2/2
✓ Branch 0 taken 8 times.
✓ Branch 1 taken 1 times.
 9 for (Index i = 0; i < (Index)(model.njoints); ++i)
50 {
51 8 os << " Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i]
52 8 << std::endl;
53 }
54
55 1 return os;
56 }
57
58 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
59 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
60 10665 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 10665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10665 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 10665 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 10665 times.
✗ Branch 11 not taken.
 10665 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 10665 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 10665 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 10665 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 10665 times.
✗ Branch 9 not taken.
 10665 assert((joint_model.nq() >= 0) && (joint_model.nv() >= 0));
76
3/6
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10665 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
✓ Branch 7 taken 10665 times.
 10665 assert(joint_model.nq() >= joint_model.nv());
77
78
2/28
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10665 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.
 10665 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 10665 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10665 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.
 10665 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 10665 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10665 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.
 10665 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 10665 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10665 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.
 10665 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 10665 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10665 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.
 10665 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 10665 times.
✗ Branch 3 not taken.
✗ Branch 4 not taken.
✓ Branch 5 taken 10665 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.
 10665 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 10665 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 10665 PINOCCHIO_CHECK_INPUT_ARGUMENT(
94 parent < (JointIndex)njoints, "The index of the parent joint is not valid.");
95
96 10665 JointIndex joint_id = (JointIndex)(njoints++);
97
98
2/4
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10665 times.
✗ Branch 6 not taken.
 10665 joints.push_back(JointModel(joint_model.derived()));
99 10665 JointModel & jmodel = joints.back();
100
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 jmodel.setIndexes(joint_id, nq, nv);
101
102
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 const int joint_nq = jmodel.nq();
103
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 const int joint_idx_q = jmodel.idx_q();
104
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 const int joint_nv = jmodel.nv();
105
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 const int joint_idx_v = jmodel.idx_v();
106
107
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10665 times.
 10665 assert(joint_idx_q >= 0);
108
1/2
✗ Branch 0 not taken.
✓ Branch 1 taken 10665 times.
 10665 assert(joint_idx_v >= 0);
109
110
2/4
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10665 times.
✗ Branch 5 not taken.
 10665 inertias.push_back(Inertia::Zero());
111
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 parents.push_back(parent);
112
1/2
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
 10665 children.push_back(IndexVector());
113
1/2
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
 10665 children[parent].push_back(joint_id);
114
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 jointPlacements.push_back(joint_placement);
115
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 names.push_back(joint_name);
116
117 10665 nq += joint_nq;
118
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 nqs.push_back(joint_nq);
119
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 idx_qs.push_back(joint_idx_q);
120 10665 nv += joint_nv;
121
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 nvs.push_back(joint_nv);
122
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 idx_vs.push_back(joint_idx_v);
123
124
3/4
✓ Branch 0 taken 10656 times.
✓ Branch 1 taken 9 times.
✓ Branch 2 taken 10656 times.
✗ Branch 3 not taken.
 10665 if (joint_nq > 0 && joint_nv > 0)
125 {
126
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 effortLimit.conservativeResize(nv);
127
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(effortLimit) = max_effort;
128
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 velocityLimit.conservativeResize(nv);
129
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(velocityLimit) = max_velocity;
130
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 lowerPositionLimit.conservativeResize(nq);
131
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointConfigSelector(lowerPositionLimit) = min_config;
132
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 upperPositionLimit.conservativeResize(nq);
133
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointConfigSelector(upperPositionLimit) = max_config;
134
135
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 armature.conservativeResize(nv);
136
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(armature).setZero();
137
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 rotorInertia.conservativeResize(nv);
138
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(rotorInertia).setZero();
139
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 rotorGearRatio.conservativeResize(nv);
140
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(rotorGearRatio).setOnes();
141
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 friction.conservativeResize(nv);
142
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(friction) = joint_friction;
143
1/2
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
 10656 damping.conservativeResize(nv);
144
2/4
✓ Branch 1 taken 10656 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 10656 times.
✗ Branch 5 not taken.
 10656 jmodel.jointVelocitySelector(damping) = joint_damping;
145 }
146
147 // Init and add joint index to its parent subtrees.
148
2/4
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 10665 times.
✗ Branch 6 not taken.
 10665 subtrees.push_back(IndexVector(1));
149 10665 subtrees[joint_id][0] = joint_id;
150
1/2
✓ Branch 1 taken 10665 times.
✗ Branch 2 not taken.
 10665 addJointIndexToParentSubtrees(joint_id);
151
152 // Init and add joint index to the supports
153
1/2
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
 10665 supports.push_back(supports[parent]);
154
1/2
✓ Branch 2 taken 10665 times.
✗ Branch 3 not taken.
 10665 supports[joint_id].push_back(joint_id);
155
156 10665 return joint_id;
157 }
158
159 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
160 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
161 8778 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/6
✓ Branch 1 taken 8778 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8778 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8778 times.
✗ Branch 8 not taken.
 8778 const VectorXs friction = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
172
3/6
✓ Branch 1 taken 8778 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 8778 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 8778 times.
✗ Branch 8 not taken.
 8778 const VectorXs damping = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0));
173
174
1/2
✓ Branch 1 taken 8778 times.
✗ Branch 2 not taken.
 8778 return addJoint(
175 parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config,
176 17556 max_config, friction, damping);
177 8778 }
178
179 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
180 typename ModelTpl<Scalar, Options, JointCollectionTpl>::JointIndex
181 231 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/2
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
 231 const VectorXs max_effort =
188
2/4
✓ Branch 2 taken 231 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 231 times.
✗ Branch 6 not taken.
 231 VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
189
1/2
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
 231 const VectorXs max_velocity =
190
2/4
✓ Branch 2 taken 231 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 231 times.
✗ Branch 6 not taken.
 231 VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max());
191
1/2
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
 231 const VectorXs min_config =
192
2/4
✓ Branch 2 taken 231 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 231 times.
✗ Branch 6 not taken.
 231 VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max());
193
1/2
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
 231 const VectorXs max_config =
194
2/4
✓ Branch 2 taken 231 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 231 times.
✗ Branch 6 not taken.
 231 VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max());
195
196
1/2
✓ Branch 1 taken 231 times.
✗ Branch 2 not taken.
 231 return addJoint(
197 parent, joint_model, joint_placement, joint_name, max_effort, max_velocity, min_config,
198 462 max_config);
199 231 }
200
201 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
202 9820 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 9820 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 9820 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 8440 times.
✓ Branch 1 taken 1380 times.
 9820 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 8440 previous_frame_index =
213
1/2
✓ Branch 3 taken 8440 times.
✗ Branch 4 not taken.
 8440 (int)getFrameId(names[parents[joint_index]], (FrameType)(JOINT | FIXED_JOINT));
214 }
215
1/2
✓ Branch 1 taken 9820 times.
✗ Branch 2 not taken.
 9820 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 9820 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 9820 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 9820 times.
✗ Branch 10 not taken.
 19640 return addFrame(Frame(
220 19640 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 ✗ ModelTpl<Scalar, Options, JointCollectionTpl>::cast() const
227 {
228 typedef ModelTpl<NewScalar, Options, JointCollectionTpl> ReturnType;
229
230 ✗ ReturnType res;
231 ✗ res.nq = nq;
232 ✗ res.nv = nv;
233 ✗ res.njoints = njoints;
234 ✗ res.nbodies = nbodies;
235 ✗ res.nframes = nframes;
236 ✗ res.parents = parents;
237 ✗ res.children = children;
238 ✗ res.names = names;
239 ✗ res.subtrees = subtrees;
240 ✗ res.supports = supports;
241 ✗ res.gravity = gravity.template cast<NewScalar>();
242 ✗ res.name = name;
243
244 ✗ res.idx_qs = idx_qs;
245 ✗ res.nqs = nqs;
246 ✗ res.idx_vs = idx_vs;
247 ✗ res.nvs = nvs;
248
249 // Eigen Vectors
250 ✗ res.armature = armature.template cast<NewScalar>();
251 ✗ res.friction = friction.template cast<NewScalar>();
252 ✗ res.damping = damping.template cast<NewScalar>();
253 ✗ res.rotorInertia = rotorInertia.template cast<NewScalar>();
254 ✗ res.rotorGearRatio = rotorGearRatio.template cast<NewScalar>();
255 ✗ res.effortLimit = effortLimit.template cast<NewScalar>();
256 ✗ res.velocityLimit = velocityLimit.template cast<NewScalar>();
257 ✗ res.lowerPositionLimit = lowerPositionLimit.template cast<NewScalar>();
258 ✗ res.upperPositionLimit = upperPositionLimit.template cast<NewScalar>();
259
260 ✗ typename ConfigVectorMap::const_iterator it;
261 ✗ 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 ✗ res.inertias.resize(inertias.size());
269 ✗ res.jointPlacements.resize(jointPlacements.size());
270 ✗ res.joints.resize(joints.size());
271
272 // copy into vectors
273 ✗ for (size_t k = 0; k < joints.size(); ++k)
274 {
275 ✗ res.inertias[k] = inertias[k].template cast<NewScalar>();
276 ✗ res.jointPlacements[k] = jointPlacements[k].template cast<NewScalar>();
277 ✗ res.joints[k] = joints[k].template cast<NewScalar>();
278 }
279
280 ✗ res.frames.resize(frames.size());
281 ✗ for (size_t k = 0; k < frames.size(); ++k)
282 {
283 ✗ res.frames[k] = frames[k].template cast<NewScalar>();
284 }
285
286 ✗ return res;
287 }
288
289 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
290 ✗ bool ModelTpl<Scalar, Options, JointCollectionTpl>::operator==(const ModelTpl & other) const
291 {
292 ✗ bool res = other.nq == nq && other.nv == nv && other.njoints == njoints
293 ✗ && other.nbodies == nbodies && other.nframes == nframes && other.parents == parents
294 ✗ && other.children == children && other.names == names && other.subtrees == subtrees
295 ✗ && other.gravity == gravity && other.name == name;
296
297 ✗ res &= other.idx_qs == idx_qs && other.nqs == nqs && other.idx_vs == idx_vs && other.nvs == nvs;
298
299 ✗ if (other.referenceConfigurations.size() != referenceConfigurations.size())
300 ✗ return false;
301
302 ✗ typename ConfigVectorMap::const_iterator it = referenceConfigurations.begin();
303 ✗ typename ConfigVectorMap::const_iterator it_other = other.referenceConfigurations.begin();
304 ✗ for (long k = 0; k < (long)referenceConfigurations.size(); ++k)
305 {
306 ✗ if (it->second.size() != it_other->second.size())
307 ✗ return false;
308 ✗ if (it->second != it_other->second)
309 ✗ return false;
310 ✗ std::advance(it, 1);
311 ✗ std::advance(it_other, 1);
312 }
313 ✗ if (other.armature.size() != armature.size())
314 ✗ return false;
315 ✗ res &= other.armature == armature;
316 ✗ if (!res)
317 ✗ return res;
318
319 ✗ if (other.friction.size() != friction.size())
320 ✗ return false;
321 ✗ res &= other.friction == friction;
322 ✗ if (!res)
323 ✗ return res;
324
325 ✗ if (other.damping.size() != damping.size())
326 ✗ return false;
327 ✗ res &= other.damping == damping;
328 ✗ if (!res)
329 ✗ return res;
330
331 ✗ if (other.rotorInertia.size() != rotorInertia.size())
332 ✗ return false;
333 ✗ res &= other.rotorInertia == rotorInertia;
334 ✗ if (!res)
335 ✗ return res;
336
337 ✗ if (other.rotorGearRatio.size() != rotorGearRatio.size())
338 ✗ return false;
339 ✗ res &= other.rotorGearRatio == rotorGearRatio;
340 ✗ if (!res)
341 ✗ return res;
342
343 ✗ if (other.effortLimit.size() != effortLimit.size())
344 ✗ return false;
345 ✗ res &= other.effortLimit == effortLimit;
346 ✗ if (!res)
347 ✗ return res;
348
349 ✗ if (other.velocityLimit.size() != velocityLimit.size())
350 ✗ return false;
351 ✗ res &= other.velocityLimit == velocityLimit;
352 ✗ if (!res)
353 ✗ return res;
354
355 ✗ if (other.lowerPositionLimit.size() != lowerPositionLimit.size())
356 ✗ return false;
357 ✗ res &= other.lowerPositionLimit == lowerPositionLimit;
358 ✗ if (!res)
359 ✗ return res;
360
361 ✗ if (other.upperPositionLimit.size() != upperPositionLimit.size())
362 ✗ return false;
363 ✗ res &= other.upperPositionLimit == upperPositionLimit;
364 ✗ if (!res)
365 ✗ return res;
366
367 ✗ for (size_t k = 1; k < inertias.size(); ++k)
368 {
369 ✗ res &= other.inertias[k] == inertias[k];
370 ✗ if (!res)
371 ✗ return res;
372 }
373
374 ✗ for (size_t k = 1; k < other.jointPlacements.size(); ++k)
375 {
376 ✗ res &= other.jointPlacements[k] == jointPlacements[k];
377 ✗ if (!res)
378 ✗ return res;
379 }
380
381 ✗ res &= other.joints == joints && other.frames == frames;
382
383 ✗ return res;
384 }
385
386 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
387 10523 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 10523 times.
✗ Branch 2 not taken.
 10523 const Inertia & iYf = Y.se3Action(body_placement);
391
1/2
✓ Branch 2 taken 10523 times.
✗ Branch 3 not taken.
 10523 inertias[joint_index] += iYf;
392 10523 nbodies++;
393 10523 }
394
395 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
396 typename ModelTpl<Scalar, Options, JointCollectionTpl>::FrameIndex
397 11096 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 8941 times.
✓ Branch 1 taken 2155 times.
 11096 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 8941 times.
✗ Branch 3 not taken.
 8941 parentFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT));
408 }
409
1/4
✗ Branch 1 not taken.
✓ Branch 2 taken 11096 times.
✗ Branch 5 not taken.
✗ Branch 6 not taken.
 11096 PINOCCHIO_CHECK_INPUT_ARGUMENT((size_t)parentFrame < frames.size(), "Frame index out of bound");
410
2/4
✓ Branch 2 taken 11096 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 11096 times.
✗ Branch 6 not taken.
 11096 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 304 ModelTpl<Scalar, Options, JointCollectionTpl>::getBodyId(const std::string & name) const
416 {
417
1/2
✓ Branch 1 taken 304 times.
✗ Branch 2 not taken.
 304 return getFrameId(name, BODY);
418 }
419
420 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
421 inline bool
422 768 ModelTpl<Scalar, Options, JointCollectionTpl>::existBodyName(const std::string & name) const
423 {
424
1/2
✓ Branch 1 taken 768 times.
✗ Branch 2 not taken.
 768 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 3207 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 3207 times.
✗ Branch 5 not taken.
 3207 it_diff_t res = std::find(names.begin(), names.end(), name) - names.begin();
433
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 3207 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 3207 PINOCCHIO_CHECK_INPUT_ARGUMENT(
434 (res < INT_MAX), "Id superior to int range. Should never happen.");
435 3207 return ModelTpl::JointIndex(res);
436 }
437
438 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
439 inline bool
440 152 ModelTpl<Scalar, Options, JointCollectionTpl>::existJointName(const std::string & name) const
441 {
442 152 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 10404 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 10404 times.
✗ Branch 5 not taken.
 10404 std::find_if(frames.begin(), frames.end(), details::FilterFrame(name, type));
452
4/10
✓ Branch 2 taken 10404 times.
✗ Branch 3 not taken.
✓ Branch 9 taken 10404 times.
✗ Branch 10 not taken.
✗ Branch 12 not taken.
✓ Branch 13 taken 10404 times.
✗ Branch 14 not taken.
✓ Branch 15 taken 10404 times.
✗ Branch 18 not taken.
✗ Branch 19 not taken.
 10404 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 10404 return FrameIndex(it - frames.begin());
457 }
458
459 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
460 12528 inline bool ModelTpl<Scalar, Options, JointCollectionTpl>::existFrame(
461 const std::string & name, const FrameType & type) const
462 {
463
1/2
✓ Branch 4 taken 12528 times.
✗ Branch 5 not taken.
 12528 return std::find_if(frames.begin(), frames.end(), details::FilterFrame(name, type))
464 25056 != frames.end();
465 }
466
467 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
468 typename ModelTpl<Scalar, Options, JointCollectionTpl>::FrameIndex
469 23706 ModelTpl<Scalar, Options, JointCollectionTpl>::addFrame(
470 const Frame & frame, const bool append_inertia)
471 {
472
1/4
✗ Branch 0 not taken.
✓ Branch 1 taken 23706 times.
✗ Branch 4 not taken.
✗ Branch 5 not taken.
 23706 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 139 times.
✓ Branch 2 taken 23567 times.
 23706 if (existFrame(frame.name, frame.type))
481 {
482 139 return getFrameId(frame.name, frame.type);
483 }
484 // else: we must add a new frames to the current stack
485 23567 frames.push_back(frame);
486
2/2
✓ Branch 0 taken 22841 times.
✓ Branch 1 taken 726 times.
 23567 if (append_inertia)
487
1/2
✓ Branch 3 taken 22841 times.
✗ Branch 4 not taken.
 22841 inertias[frame.parentJoint] += frame.placement.act(frame.inertia);
488 23567 nframes++;
489 23567 return FrameIndex(nframes - 1);
490 }
491
492 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
493 10665 void ModelTpl<Scalar, Options, JointCollectionTpl>::addJointIndexToParentSubtrees(
494 const JointIndex joint_id)
495 {
496
2/2
✓ Branch 2 taken 43501 times.
✓ Branch 3 taken 10665 times.
 54166 for (JointIndex parent = parents[joint_id]; parent > 0; parent = parents[parent])
497 43501 subtrees[parent].push_back(joint_id);
498
499 // Also add joint_id to the universe
500 10665 subtrees[0].push_back(joint_id);
501 10665 }
502
503 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
504 ✗ std::vector<bool> ModelTpl<Scalar, Options, JointCollectionTpl>::hasConfigurationLimit()
505 {
506 ✗ std::vector<bool> vec;
507 ✗ for (Index i = 1; i < (Index)(njoints); ++i)
508 {
509 ✗ const std::vector<bool> & cf_limits = joints[i].hasConfigurationLimit();
510 ✗ vec.insert(vec.end(), cf_limits.begin(), cf_limits.end());
511 }
512 ✗ return vec;
513 }
514
515 template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
516 ✗ std::vector<bool> ModelTpl<Scalar, Options, JointCollectionTpl>::hasConfigurationLimitInTangent()
517 {
518 ✗ std::vector<bool> vec;
519 ✗ for (Index i = 1; i < (Index)(njoints); ++i)
520 {
521 ✗ const std::vector<bool> & cf_limits = joints[i].hasConfigurationLimitInTangent();
522 ✗ vec.insert(vec.end(), cf_limits.begin(), cf_limits.end());
523 }
524 ✗ return vec;
525 }
526
527 } // namespace pinocchio
528
529 /// @endcond
530
531 #endif // ifndef __pinocchio_multibody_model_hxx__
532