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 |