| GCC Code Coverage Report | |||||||||||||||||||||
|
|||||||||||||||||||||
| Line | Branch | Exec | Source |
1 |
// |
||
2 |
// Copyright (c) 2015-2020 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/spatial/fwd.hpp" |
||
10 |
#include "pinocchio/utils/string-generator.hpp" |
||
11 |
#include "pinocchio/multibody/liegroup/liegroup-algo.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 |
64665 |
FilterFrame(const std::string& name, const FrameType & typeMask) |
|
25 |
64665 |
: name(name), typeMask(typeMask) |
|
26 |
64665 |
{} |
|
27 |
|||
28 |
template<typename Scalar, int Options> |
||
29 |
2435356 |
bool operator()(const FrameTpl<Scalar,Options> & frame) const |
|
30 |
✓✓✓✓ |
2435356 |
{ return (typeMask & frame.type) && (name == frame.name); } |
31 |
|||
32 |
}; |
||
33 |
} |
||
34 |
|||
35 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
36 |
const typename ModelTpl<Scalar,Options,JointCollectionTpl>:: |
||
37 |
Vector3 ModelTpl<Scalar,Options,JointCollectionTpl>::gravity981((Scalar)0,(Scalar)0,(Scalar)-9.81); |
||
38 |
|||
39 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
40 |
2 |
inline std::ostream& operator<<(std::ostream & os, |
|
41 |
const ModelTpl<Scalar,Options,JointCollectionTpl> & model) |
||
42 |
{ |
||
43 |
typedef typename ModelTpl<Scalar,Options,JointCollectionTpl>::Index Index; |
||
44 |
|||
45 |
2 |
os << "Nb joints = " << model.njoints << " (nq="<< model.nq<<",nv="<<model.nv<<")" << std::endl; |
|
46 |
✓✓ | 13 |
for(Index i=0;i<(Index)(model.njoints);++i) |
47 |
{ |
||
48 |
11 |
os << " Joint " << i << " " << model.names[i] << ": parent=" << model.parents[i] << std::endl; |
|
49 |
} |
||
50 |
|||
51 |
2 |
return os; |
|
52 |
} |
||
53 |
|||
54 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
55 |
typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex |
||
56 |
8316 |
ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent, |
|
57 |
const JointModel & joint_model, |
||
58 |
const SE3 & joint_placement, |
||
59 |
const std::string & joint_name, |
||
60 |
const VectorXs & max_effort, |
||
61 |
const VectorXs & max_velocity, |
||
62 |
const VectorXs & min_config, |
||
63 |
const VectorXs & max_config, |
||
64 |
const VectorXs & joint_friction, |
||
65 |
const VectorXs & joint_damping |
||
66 |
) |
||
67 |
{ |
||
68 |
✓✗✓✗ ✓✗✓✗ |
8316 |
assert( (njoints==(int)joints.size())&&(njoints==(int)inertias.size()) |
69 |
&&(njoints==(int)parents.size())&&(njoints==(int)jointPlacements.size()) ); |
||
70 |
✓✗✓✗ ✓✗✓✗ |
8316 |
assert((joint_model.nq()>=0) && (joint_model.nv()>=0)); |
71 |
✓✗✓✗ ✗✓ |
8316 |
assert(joint_model.nq() >= joint_model.nv()); |
72 |
|||
73 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
8316 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(max_effort.size(),joint_model.nv(),"The joint maximum effort vector is not of right size"); |
74 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
8316 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(max_velocity.size(),joint_model.nv(),"The joint maximum velocity vector is not of right size"); |
75 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
8316 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(min_config.size(),joint_model.nq(),"The joint lower configuration bound is not of right size"); |
76 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
8316 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(max_config.size(),joint_model.nq(),"The joint upper configuration bound is not of right size"); |
77 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
8316 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_friction.size(),joint_model.nv(),"The joint friction vector is not of right size"); |
78 |
✓✗✓✗ ✗✓✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ ✗✗✗✗ |
8316 |
PINOCCHIO_CHECK_ARGUMENT_SIZE(joint_damping.size(),joint_model.nv(),"The joint damping vector is not of right size"); |
79 |
✗✓✗✗ |
8316 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(parent < (JointIndex)njoints, "The index of the parent joint is not valid."); |
80 |
|||
81 |
8316 |
JointIndex idx = (JointIndex)(njoints++); |
|
82 |
|||
83 |
✓✗✓✗ |
8316 |
joints .push_back(JointModel(joint_model.derived())); |
84 |
// JointModelDerived & jmodel = boost::get<JointModelDerived>(joints.back()); |
||
85 |
8316 |
JointModel & jmodel = joints.back(); |
|
86 |
✓✗ | 8316 |
jmodel.setIndexes(idx,nq,nv); |
87 |
|||
88 |
✓✗ | 8316 |
const int joint_nq = jmodel.nq(); |
89 |
✓✗ | 8316 |
const int joint_idx_q = jmodel.idx_q(); |
90 |
✓✗ | 8316 |
const int joint_nv = jmodel.nv(); |
91 |
✓✗ | 8316 |
const int joint_idx_v = jmodel.idx_v(); |
92 |
|||
93 |
✗✓ | 8316 |
assert(joint_idx_q >= 0); |
94 |
✗✓ | 8316 |
assert(joint_idx_v >= 0); |
95 |
|||
96 |
✓✗✓✗ |
8316 |
inertias .push_back(Inertia::Zero()); |
97 |
✓✗ | 8316 |
parents .push_back(parent); |
98 |
✓✗ | 8316 |
jointPlacements.push_back(joint_placement); |
99 |
✓✗ | 8316 |
names .push_back(joint_name); |
100 |
|||
101 |
|||
102 |
✓✗✓✗ |
8316 |
nq += joint_nq; nqs.push_back(joint_nq); idx_qs.push_back(joint_idx_q); |
103 |
✓✗✓✗ |
8316 |
nv += joint_nv; nvs.push_back(joint_nv); idx_vs.push_back(joint_idx_v); |
104 |
|||
105 |
✓✓✓✗ |
8316 |
if(joint_nq > 0 && joint_nv > 0) |
106 |
{ |
||
107 |
✓✗ | 8307 |
effortLimit.conservativeResize(nv); |
108 |
✓✗✓✗ |
8307 |
jmodel.jointVelocitySelector(effortLimit) = max_effort; |
109 |
✓✗ | 8307 |
velocityLimit.conservativeResize(nv); |
110 |
✓✗✓✗ |
8307 |
jmodel.jointVelocitySelector(velocityLimit) = max_velocity; |
111 |
✓✗ | 8307 |
lowerPositionLimit.conservativeResize(nq); |
112 |
✓✗✓✗ |
8307 |
jmodel.jointConfigSelector(lowerPositionLimit) = min_config; |
113 |
✓✗ | 8307 |
upperPositionLimit.conservativeResize(nq); |
114 |
✓✗✓✗ |
8307 |
jmodel.jointConfigSelector(upperPositionLimit) = max_config; |
115 |
|||
116 |
✓✗ | 8307 |
rotorInertia.conservativeResize(nv); |
117 |
✓✗✓✗ |
8307 |
jmodel.jointVelocitySelector(rotorInertia).setZero(); |
118 |
✓✗ | 8307 |
rotorGearRatio.conservativeResize(nv); |
119 |
✓✗✓✗ |
8307 |
jmodel.jointVelocitySelector(rotorGearRatio).setOnes(); |
120 |
✓✗ | 8307 |
friction.conservativeResize(nv); |
121 |
✓✗✓✗ |
8307 |
jmodel.jointVelocitySelector(friction) = joint_friction; |
122 |
✓✗ | 8307 |
damping.conservativeResize(nv); |
123 |
✓✗✓✗ |
8307 |
jmodel.jointVelocitySelector(damping) = joint_damping; |
124 |
} |
||
125 |
|||
126 |
// Init and add joint index to its parent subtrees. |
||
127 |
✓✗✓✗ |
8316 |
subtrees.push_back(IndexVector(1)); |
128 |
8316 |
subtrees[idx][0] = idx; |
|
129 |
✓✗ | 8316 |
addJointIndexToParentSubtrees(idx); |
130 |
|||
131 |
// Init and add joint index to the supports |
||
132 |
✓✗ | 8316 |
supports.push_back(supports[parent]); |
133 |
✓✗ | 8316 |
supports[idx].push_back(idx); |
134 |
|||
135 |
8316 |
return idx; |
|
136 |
} |
||
137 |
|||
138 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
139 |
typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex |
||
140 |
6775 |
ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent, |
|
141 |
const JointModel & joint_model, |
||
142 |
const SE3 & joint_placement, |
||
143 |
const std::string & joint_name, |
||
144 |
const VectorXs & max_effort, |
||
145 |
const VectorXs & max_velocity, |
||
146 |
const VectorXs & min_config, |
||
147 |
const VectorXs & max_config) |
||
148 |
{ |
||
149 |
✓✗✓✗ ✓✗ |
13550 |
const VectorXs friction = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0)); |
150 |
✓✗✓✗ ✓✗ |
6775 |
const VectorXs damping = VectorXs::Constant(joint_model.nv(), static_cast<Scalar>(0)); |
151 |
|||
152 |
return addJoint(parent, joint_model, |
||
153 |
joint_placement, joint_name, |
||
154 |
max_effort, max_velocity, min_config, max_config, |
||
155 |
✓✗ | 13550 |
friction, damping); |
156 |
} |
||
157 |
|||
158 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
159 |
typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex |
||
160 |
178 |
ModelTpl<Scalar,Options,JointCollectionTpl>::addJoint(const JointIndex parent, |
|
161 |
const JointModel & joint_model, |
||
162 |
const SE3 & joint_placement, |
||
163 |
const std::string & joint_name) |
||
164 |
{ |
||
165 |
✓✗✓✗ ✓✗ |
356 |
const VectorXs max_effort = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max()); |
166 |
✓✗✓✗ ✓✗ |
356 |
const VectorXs max_velocity = VectorXs::Constant(joint_model.nv(), std::numeric_limits<Scalar>::max()); |
167 |
✓✗✓✗ ✓✗ |
356 |
const VectorXs min_config = VectorXs::Constant(joint_model.nq(), -std::numeric_limits<Scalar>::max()); |
168 |
✓✗✓✗ ✓✗ |
178 |
const VectorXs max_config = VectorXs::Constant(joint_model.nq(), std::numeric_limits<Scalar>::max()); |
169 |
|||
170 |
return addJoint(parent, joint_model, joint_placement, joint_name, |
||
171 |
✓✗ | 356 |
max_effort, max_velocity, min_config, max_config); |
172 |
} |
||
173 |
|||
174 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
175 |
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex |
||
176 |
7588 |
ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
177 |
addJointFrame(const JointIndex & joint_index, |
||
178 |
int previous_frame_index) |
||
179 |
{ |
||
180 |
✗✓✗✗ |
7588 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(joint_index < joints.size(), |
181 |
"The joint index is larger than the number of joints in the model."); |
||
182 |
✓✓ | 7588 |
if(previous_frame_index < 0) |
183 |
{ |
||
184 |
// FIXED_JOINT is required because the parent can be the universe and its |
||
185 |
// type is FIXED_JOINT |
||
186 |
✓✗ | 6512 |
previous_frame_index = (int)getFrameId(names[parents[joint_index]], (FrameType)(JOINT | FIXED_JOINT)); |
187 |
} |
||
188 |
✓✗ | 7588 |
assert((size_t)previous_frame_index < frames.size() && "Frame index out of bound"); |
189 |
|||
190 |
// Add a the joint frame attached to itself to the frame vector - redundant information but useful. |
||
191 |
✓✗✓✗ ✓✗ |
7588 |
return addFrame(Frame(names[joint_index],joint_index,(FrameIndex)previous_frame_index,SE3::Identity(),JOINT)); |
192 |
} |
||
193 |
|||
194 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
195 |
8206 |
inline void ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
196 |
appendBodyToJoint(const typename ModelTpl::JointIndex joint_index, |
||
197 |
const Inertia & Y, |
||
198 |
const SE3 & body_placement) |
||
199 |
{ |
||
200 |
✓✗ | 8206 |
const Inertia & iYf = Y.se3Action(body_placement); |
201 |
✓✗ | 8206 |
inertias[joint_index] += iYf; |
202 |
8206 |
nbodies++; |
|
203 |
8206 |
} |
|
204 |
|||
205 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
206 |
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex |
||
207 |
8283 |
ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
208 |
addBodyFrame(const std::string & body_name, |
||
209 |
const JointIndex & parentJoint, |
||
210 |
const SE3 & body_placement, |
||
211 |
int previousFrame) |
||
212 |
{ |
||
213 |
✓✓ | 8283 |
if(previousFrame < 0) { |
214 |
// FIXED_JOINT is required because the parent can be the universe and its |
||
215 |
// type is FIXED_JOINT |
||
216 |
✓✗ | 6613 |
previousFrame = (int)getFrameId(names[parentJoint], (FrameType)(JOINT | FIXED_JOINT)); |
217 |
} |
||
218 |
✓✗ | 8283 |
assert((size_t)previousFrame < frames.size() && "Frame index out of bound"); |
219 |
✓✗✓✗ |
8283 |
return addFrame(Frame(body_name, parentJoint, (FrameIndex)previousFrame, body_placement, BODY)); |
220 |
} |
||
221 |
|||
222 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
223 |
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex |
||
224 |
2203 |
ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
225 |
getBodyId(const std::string & name) const |
||
226 |
{ |
||
227 |
✓✗ | 2203 |
return getFrameId(name, BODY); |
228 |
} |
||
229 |
|||
230 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
231 |
3990 |
inline bool ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
232 |
existBodyName(const std::string & name) const |
||
233 |
{ |
||
234 |
✓✗ | 3990 |
return existFrame(name, BODY); |
235 |
} |
||
236 |
|||
237 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
238 |
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::JointIndex |
||
239 |
7779 |
ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
240 |
getJointId(const std::string & name) const |
||
241 |
{ |
||
242 |
typedef std::vector<std::string>::iterator::difference_type it_diff_t; |
||
243 |
✓✗ | 7779 |
it_diff_t res = std::find(names.begin(),names.end(),name) - names.begin(); |
244 |
✗✓ | 7779 |
assert((res<INT_MAX) && "Id superior to int range. Should never happen."); |
245 |
7779 |
return ModelTpl::JointIndex(res); |
|
246 |
} |
||
247 |
|||
248 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
249 |
801 |
inline bool ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
250 |
existJointName(const std::string & name) const |
||
251 |
{ |
||
252 |
801 |
return (names.end() != std::find(names.begin(),names.end(),name)); |
|
253 |
} |
||
254 |
|||
255 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
256 |
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex |
||
257 |
28968 |
ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
258 |
getFrameId(const std::string & name, const FrameType & type) const |
||
259 |
{ |
||
260 |
typename PINOCCHIO_ALIGNED_STD_VECTOR(Frame)::const_iterator it |
||
261 |
✓✗ | 28968 |
= std::find_if(frames.begin() |
262 |
,frames.end() |
||
263 |
,details::FilterFrame(name, type)); |
||
264 |
✓✓✓✗ ✓✓✓✓ ✓✗ |
28968 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(((it == frames.end() || (std::find_if(boost::next(it), frames.end(), details::FilterFrame(name, type)) == frames.end()))), |
265 |
"Several frames match the filter - please specify the FrameType"); |
||
266 |
28967 |
return FrameIndex(it - frames.begin()); |
|
267 |
} |
||
268 |
|||
269 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
270 |
34032 |
inline bool ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
271 |
existFrame(const std::string & name, const FrameType & type) const |
||
272 |
{ |
||
273 |
✓✗ | 34032 |
return std::find_if(frames.begin(), frames.end(), |
274 |
68064 |
details::FilterFrame(name, type)) != frames.end(); |
|
275 |
} |
||
276 |
|||
277 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
278 |
inline typename ModelTpl<Scalar,Options,JointCollectionTpl>::FrameIndex |
||
279 |
26431 |
ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
280 |
addFrame(const Frame & frame, const bool append_inertia) |
||
281 |
{ |
||
282 |
✗✓✗✗ |
26431 |
PINOCCHIO_CHECK_INPUT_ARGUMENT(frame.parent < (JointIndex)njoints, |
283 |
"The index of the parent joint is not valid."); |
||
284 |
|||
285 |
// TODO: fix it |
||
286 |
// PINOCCHIO_CHECK_INPUT_ARGUMENT(frame.inertia.isValid(), |
||
287 |
// "The input inertia is not valid.") |
||
288 |
|||
289 |
// Check if the frame.name exists with the same type |
||
290 |
✓✓ | 26431 |
if(existFrame(frame.name,frame.type)) |
291 |
1 |
return getFrameId(frame.name,frame.type); |
|
292 |
|||
293 |
26430 |
frames.push_back(frame); |
|
294 |
✓✓ | 26430 |
if(append_inertia) |
295 |
✓✗ | 25296 |
inertias[frame.parent] += frame.placement.act(frame.inertia); |
296 |
26430 |
nframes++; |
|
297 |
26430 |
return FrameIndex(nframes - 1); |
|
298 |
} |
||
299 |
|||
300 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
301 |
8316 |
inline void ModelTpl<Scalar,Options,JointCollectionTpl>:: |
|
302 |
addJointIndexToParentSubtrees(const JointIndex joint_id) |
||
303 |
{ |
||
304 |
✓✓ | 42037 |
for(JointIndex parent = parents[joint_id]; parent>0; parent = parents[parent]) |
305 |
33721 |
subtrees[parent].push_back(joint_id); |
|
306 |
|||
307 |
// Also add joint_id to the universe |
||
308 |
8316 |
subtrees[0].push_back(joint_id); |
|
309 |
8316 |
} |
|
310 |
|||
311 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
312 |
1 |
std::vector<bool> ModelTpl<Scalar,Options,JointCollectionTpl>::hasConfigurationLimit() |
|
313 |
{ |
||
314 |
1 |
std::vector<bool> vec; |
|
315 |
✓✓ | 4 |
for(Index i=1;i<(Index)(njoints);++i) |
316 |
{ |
||
317 |
✓✗ | 3 |
const std::vector<bool> & cf_limits = joints[i].hasConfigurationLimit(); |
318 |
✓✗ | 3 |
vec.insert(vec.end(), |
319 |
cf_limits.begin(), |
||
320 |
cf_limits.end()); |
||
321 |
} |
||
322 |
1 |
return vec; |
|
323 |
} |
||
324 |
|||
325 |
template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl> |
||
326 |
1 |
std::vector<bool> ModelTpl<Scalar,Options,JointCollectionTpl>::hasConfigurationLimitInTangent() |
|
327 |
{ |
||
328 |
1 |
std::vector<bool> vec; |
|
329 |
✓✓ | 4 |
for(Index i=1;i<(Index)(njoints);++i) |
330 |
{ |
||
331 |
✓✗ | 3 |
const std::vector<bool> & cf_limits = joints[i].hasConfigurationLimitInTangent(); |
332 |
✓✗ | 3 |
vec.insert(vec.end(), |
333 |
cf_limits.begin(), |
||
334 |
cf_limits.end()); |
||
335 |
} |
||
336 |
1 |
return vec; |
|
337 |
} |
||
338 |
|||
339 |
} // namespace pinocchio |
||
340 |
|||
341 |
/// @endcond |
||
342 |
|||
343 |
#endif // ifndef __pinocchio_multibody_model_hxx__ |
| Generated by: GCOVR (Version 4.2) |