1 #include "MultiBodyTreeImpl.hpp"
2
3 namespace btInverseDynamics
4 {
MultiBodyImpl(int num_bodies_,int num_dofs_)5 MultiBodyTree::MultiBodyImpl::MultiBodyImpl(int num_bodies_, int num_dofs_)
6 : m_num_bodies(num_bodies_), m_num_dofs(num_dofs_)
7 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
8 ,
9 m_m3x(3, m_num_dofs)
10 #endif
11 {
12 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
13 resize(m_m3x, m_num_dofs);
14 #endif
15 m_body_list.resize(num_bodies_);
16 m_parent_index.resize(num_bodies_);
17 m_child_indices.resize(num_bodies_);
18 m_user_int.resize(num_bodies_);
19 m_user_ptr.resize(num_bodies_);
20
21 m_world_gravity(0) = 0.0;
22 m_world_gravity(1) = 0.0;
23 m_world_gravity(2) = -9.8;
24 }
25
jointTypeToString(const JointType & type) const26 const char *MultiBodyTree::MultiBodyImpl::jointTypeToString(const JointType &type) const
27 {
28 switch (type)
29 {
30 case FIXED:
31 return "fixed";
32 case REVOLUTE:
33 return "revolute";
34 case PRISMATIC:
35 return "prismatic";
36 case FLOATING:
37 return "floating";
38 case SPHERICAL:
39 return "spherical";
40 }
41 return "error: invalid";
42 }
43
indent(const int & level)44 inline void indent(const int &level)
45 {
46 for (int j = 0; j < level; j++)
47 id_printf(" "); // indent
48 }
49
printTree()50 void MultiBodyTree::MultiBodyImpl::printTree()
51 {
52 id_printf("body %.2d[%s]: root\n", 0, jointTypeToString(m_body_list[0].m_joint_type));
53 printTree(0, 0);
54 }
55
printTreeData()56 void MultiBodyTree::MultiBodyImpl::printTreeData()
57 {
58 for (idArrayIdx i = 0; i < m_body_list.size(); i++)
59 {
60 RigidBody &body = m_body_list[i];
61 id_printf("body: %d\n", static_cast<int>(i));
62 id_printf("type: %s\n", jointTypeToString(body.m_joint_type));
63 id_printf("q_index= %d\n", body.m_q_index);
64 id_printf("Jac_JR= [%f;%f;%f]\n", body.m_Jac_JR(0), body.m_Jac_JR(1), body.m_Jac_JR(2));
65 id_printf("Jac_JT= [%f;%f;%f]\n", body.m_Jac_JT(0), body.m_Jac_JT(1), body.m_Jac_JT(2));
66
67 id_printf("mass = %f\n", body.m_mass);
68 id_printf("mass * com = [%f %f %f]\n", body.m_body_mass_com(0), body.m_body_mass_com(1),
69 body.m_body_mass_com(2));
70 id_printf(
71 "I_o= [%f %f %f;\n"
72 " %f %f %f;\n"
73 " %f %f %f]\n",
74 body.m_body_I_body(0, 0), body.m_body_I_body(0, 1), body.m_body_I_body(0, 2),
75 body.m_body_I_body(1, 0), body.m_body_I_body(1, 1), body.m_body_I_body(1, 2),
76 body.m_body_I_body(2, 0), body.m_body_I_body(2, 1), body.m_body_I_body(2, 2));
77
78 id_printf("parent_pos_parent_body_ref= [%f %f %f]\n", body.m_parent_pos_parent_body_ref(0),
79 body.m_parent_pos_parent_body_ref(1), body.m_parent_pos_parent_body_ref(2));
80 }
81 }
bodyNumDoFs(const JointType & type) const82 int MultiBodyTree::MultiBodyImpl::bodyNumDoFs(const JointType &type) const
83 {
84 switch (type)
85 {
86 case FIXED:
87 return 0;
88 case REVOLUTE:
89 case PRISMATIC:
90 return 1;
91 case FLOATING:
92 return 6;
93 case SPHERICAL:
94 return 3;
95 }
96 bt_id_error_message("unknown joint type %d\n", type);
97 return 0;
98 }
99
printTree(int index,int indentation)100 void MultiBodyTree::MultiBodyImpl::printTree(int index, int indentation)
101 {
102 // this is adapted from URDF2Bullet.
103 // TODO: fix this and print proper graph (similar to git --log --graph)
104 int num_children = m_child_indices[index].size();
105
106 indentation += 2;
107 int count = 0;
108
109 for (int i = 0; i < num_children; i++)
110 {
111 int child_index = m_child_indices[index][i];
112 indent(indentation);
113 id_printf("body %.2d[%s]: %.2d is child no. %d (qi= %d .. %d) \n", index,
114 jointTypeToString(m_body_list[index].m_joint_type), child_index, (count++) + 1,
115 m_body_list[index].m_q_index,
116 m_body_list[index].m_q_index + bodyNumDoFs(m_body_list[index].m_joint_type));
117 // first grandchild
118 printTree(child_index, indentation);
119 }
120 }
121
setGravityInWorldFrame(const vec3 & gravity)122 int MultiBodyTree::MultiBodyImpl::setGravityInWorldFrame(const vec3 &gravity)
123 {
124 m_world_gravity = gravity;
125 return 0;
126 }
127
generateIndexSets()128 int MultiBodyTree::MultiBodyImpl::generateIndexSets()
129 {
130 m_body_revolute_list.resize(0);
131 m_body_prismatic_list.resize(0);
132 int q_index = 0;
133 for (idArrayIdx i = 0; i < m_body_list.size(); i++)
134 {
135 RigidBody &body = m_body_list[i];
136 body.m_q_index = -1;
137 switch (body.m_joint_type)
138 {
139 case REVOLUTE:
140 m_body_revolute_list.push_back(i);
141 body.m_q_index = q_index;
142 q_index++;
143 break;
144 case PRISMATIC:
145 m_body_prismatic_list.push_back(i);
146 body.m_q_index = q_index;
147 q_index++;
148 break;
149 case FIXED:
150 // do nothing
151 break;
152 case FLOATING:
153 m_body_floating_list.push_back(i);
154 body.m_q_index = q_index;
155 q_index += 6;
156 break;
157 case SPHERICAL:
158 m_body_spherical_list.push_back(i);
159 body.m_q_index = q_index;
160 q_index += 3;
161 break;
162 default:
163 bt_id_error_message("unsupported joint type %d\n", body.m_joint_type);
164 return -1;
165 }
166 }
167 // sanity check
168 if (q_index != m_num_dofs)
169 {
170 bt_id_error_message("internal error, q_index= %d but num_dofs %d\n", q_index, m_num_dofs);
171 return -1;
172 }
173
174 m_child_indices.resize(m_body_list.size());
175
176 for (idArrayIdx child = 1; child < m_parent_index.size(); child++)
177 {
178 const int &parent = m_parent_index[child];
179 if (parent >= 0 && parent < (static_cast<int>(m_parent_index.size()) - 1))
180 {
181 m_child_indices[parent].push_back(child);
182 }
183 else
184 {
185 if (-1 == parent)
186 {
187 // multiple bodies are directly linked to the environment, ie, not a single root
188 bt_id_error_message("building index sets parent(%zu)= -1 (multiple roots)\n", child);
189 }
190 else
191 {
192 // should never happen
193 bt_id_error_message(
194 "building index sets. parent_index[%zu]= %d, but m_parent_index.size()= %d\n",
195 child, parent, static_cast<int>(m_parent_index.size()));
196 }
197 return -1;
198 }
199 }
200
201 return 0;
202 }
203
calculateStaticData()204 void MultiBodyTree::MultiBodyImpl::calculateStaticData()
205 {
206 // relative kinematics that are not a function of q, u, dot_u
207 for (idArrayIdx i = 0; i < m_body_list.size(); i++)
208 {
209 RigidBody &body = m_body_list[i];
210 switch (body.m_joint_type)
211 {
212 case REVOLUTE:
213 body.m_parent_vel_rel(0) = 0;
214 body.m_parent_vel_rel(1) = 0;
215 body.m_parent_vel_rel(2) = 0;
216 body.m_parent_acc_rel(0) = 0;
217 body.m_parent_acc_rel(1) = 0;
218 body.m_parent_acc_rel(2) = 0;
219 body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
220 break;
221 case PRISMATIC:
222 body.m_body_T_parent = body.m_body_T_parent_ref;
223 body.m_parent_Jac_JT = body.m_body_T_parent_ref.transpose() * body.m_Jac_JT;
224 body.m_body_ang_vel_rel(0) = 0;
225 body.m_body_ang_vel_rel(1) = 0;
226 body.m_body_ang_vel_rel(2) = 0;
227 body.m_body_ang_acc_rel(0) = 0;
228 body.m_body_ang_acc_rel(1) = 0;
229 body.m_body_ang_acc_rel(2) = 0;
230 break;
231 case FIXED:
232 body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
233 body.m_body_T_parent = body.m_body_T_parent_ref;
234 body.m_body_ang_vel_rel(0) = 0;
235 body.m_body_ang_vel_rel(1) = 0;
236 body.m_body_ang_vel_rel(2) = 0;
237 body.m_parent_vel_rel(0) = 0;
238 body.m_parent_vel_rel(1) = 0;
239 body.m_parent_vel_rel(2) = 0;
240 body.m_body_ang_acc_rel(0) = 0;
241 body.m_body_ang_acc_rel(1) = 0;
242 body.m_body_ang_acc_rel(2) = 0;
243 body.m_parent_acc_rel(0) = 0;
244 body.m_parent_acc_rel(1) = 0;
245 body.m_parent_acc_rel(2) = 0;
246 break;
247 case FLOATING:
248 // no static data
249 break;
250 case SPHERICAL:
251 //todo: review
252 body.m_parent_pos_parent_body = body.m_parent_pos_parent_body_ref;
253 body.m_parent_vel_rel(0) = 0;
254 body.m_parent_vel_rel(1) = 0;
255 body.m_parent_vel_rel(2) = 0;
256 body.m_parent_acc_rel(0) = 0;
257 body.m_parent_acc_rel(1) = 0;
258 body.m_parent_acc_rel(2) = 0;
259 break;
260 }
261
262 // resize & initialize jacobians to zero.
263 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
264 body.m_body_dot_Jac_T_u(0) = 0.0;
265 body.m_body_dot_Jac_T_u(1) = 0.0;
266 body.m_body_dot_Jac_T_u(2) = 0.0;
267 body.m_body_dot_Jac_R_u(0) = 0.0;
268 body.m_body_dot_Jac_R_u(1) = 0.0;
269 body.m_body_dot_Jac_R_u(2) = 0.0;
270 resize(body.m_body_Jac_T, m_num_dofs);
271 resize(body.m_body_Jac_R, m_num_dofs);
272 body.m_body_Jac_T.setZero();
273 body.m_body_Jac_R.setZero();
274 #endif //
275 }
276 }
277
calculateInverseDynamics(const vecx & q,const vecx & u,const vecx & dot_u,vecx * joint_forces)278 int MultiBodyTree::MultiBodyImpl::calculateInverseDynamics(const vecx &q, const vecx &u,
279 const vecx &dot_u, vecx *joint_forces)
280 {
281 if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs ||
282 joint_forces->size() != m_num_dofs)
283 {
284 bt_id_error_message(
285 "wrong vector dimension. system has %d DOFs,\n"
286 "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d, dim(joint_forces)= %d\n",
287 m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
288 static_cast<int>(dot_u.size()), static_cast<int>(joint_forces->size()));
289 return -1;
290 }
291 // 1. relative kinematics
292 if (-1 == calculateKinematics(q, u, dot_u, POSITION_VELOCITY_ACCELERATION))
293 {
294 bt_id_error_message("error in calculateKinematics\n");
295 return -1;
296 }
297 // 2. update contributions to equations of motion for every body.
298 for (idArrayIdx i = 0; i < m_body_list.size(); i++)
299 {
300 RigidBody &body = m_body_list[i];
301 // 3.4 update dynamic terms (rate of change of angular & linear momentum)
302 body.m_eom_lhs_rotational =
303 body.m_body_I_body * body.m_body_ang_acc + body.m_body_mass_com.cross(body.m_body_acc) +
304 body.m_body_ang_vel.cross(body.m_body_I_body * body.m_body_ang_vel) -
305 body.m_body_moment_user;
306 body.m_eom_lhs_translational =
307 body.m_body_ang_acc.cross(body.m_body_mass_com) + body.m_mass * body.m_body_acc +
308 body.m_body_ang_vel.cross(body.m_body_ang_vel.cross(body.m_body_mass_com)) -
309 body.m_body_force_user;
310 }
311
312 // 3. calculate full set of forces at parent joint
313 // (not directly calculating the joint force along the free direction
314 // simplifies inclusion of fixed joints.
315 // An alternative would be to fuse bodies in a pre-processing step,
316 // but that would make changing masses online harder (eg, payload masses
317 // added with fixed joints to a gripper)
318 // Also, this enables adding zero weight bodies as a way to calculate frame poses
319 // for force elements, etc.
320
321 for (int body_idx = m_body_list.size() - 1; body_idx >= 0; body_idx--)
322 {
323 // sum of forces and moments acting on this body from its children
324 vec3 sum_f_children;
325 vec3 sum_m_children;
326 setZero(sum_f_children);
327 setZero(sum_m_children);
328 for (idArrayIdx child_list_idx = 0; child_list_idx < m_child_indices[body_idx].size();
329 child_list_idx++)
330 {
331 const RigidBody &child = m_body_list[m_child_indices[body_idx][child_list_idx]];
332 vec3 child_joint_force_in_this_frame =
333 child.m_body_T_parent.transpose() * child.m_force_at_joint;
334 sum_f_children -= child_joint_force_in_this_frame;
335 sum_m_children -= child.m_body_T_parent.transpose() * child.m_moment_at_joint +
336 child.m_parent_pos_parent_body.cross(child_joint_force_in_this_frame);
337 }
338 RigidBody &body = m_body_list[body_idx];
339
340 body.m_force_at_joint = body.m_eom_lhs_translational - sum_f_children;
341 body.m_moment_at_joint = body.m_eom_lhs_rotational - sum_m_children;
342 }
343
344 // 4. Calculate Joint forces.
345 // These are the components of force_at_joint/moment_at_joint
346 // in the free directions given by Jac_JT/Jac_JR
347 // 4.1 revolute joints
348 for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
349 {
350 RigidBody &body = m_body_list[m_body_revolute_list[i]];
351 // (*joint_forces)(body.m_q_index) = body.m_Jac_JR.transpose() * body.m_moment_at_joint;
352 (*joint_forces)(body.m_q_index) = body.m_Jac_JR.dot(body.m_moment_at_joint);
353 }
354 // 4.2 for prismatic joints
355 for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
356 {
357 RigidBody &body = m_body_list[m_body_prismatic_list[i]];
358 // (*joint_forces)(body.m_q_index) = body.m_Jac_JT.transpose() * body.m_force_at_joint;
359 (*joint_forces)(body.m_q_index) = body.m_Jac_JT.dot(body.m_force_at_joint);
360 }
361 // 4.3 floating bodies (6-DoF joints)
362 for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
363 {
364 RigidBody &body = m_body_list[m_body_floating_list[i]];
365 (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
366 (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
367 (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
368
369 (*joint_forces)(body.m_q_index + 3) = body.m_force_at_joint(0);
370 (*joint_forces)(body.m_q_index + 4) = body.m_force_at_joint(1);
371 (*joint_forces)(body.m_q_index + 5) = body.m_force_at_joint(2);
372 }
373
374 // 4.4 spherical bodies (3-DoF joints)
375 for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
376 {
377 //todo: review
378 RigidBody &body = m_body_list[m_body_spherical_list[i]];
379 (*joint_forces)(body.m_q_index + 0) = body.m_moment_at_joint(0);
380 (*joint_forces)(body.m_q_index + 1) = body.m_moment_at_joint(1);
381 (*joint_forces)(body.m_q_index + 2) = body.m_moment_at_joint(2);
382 }
383 return 0;
384 }
385
calculateKinematics(const vecx & q,const vecx & u,const vecx & dot_u,const KinUpdateType type)386 int MultiBodyTree::MultiBodyImpl::calculateKinematics(const vecx &q, const vecx &u, const vecx &dot_u,
387 const KinUpdateType type)
388 {
389 if (q.size() != m_num_dofs || u.size() != m_num_dofs || dot_u.size() != m_num_dofs)
390 {
391 bt_id_error_message(
392 "wrong vector dimension. system has %d DOFs,\n"
393 "but dim(q)= %d, dim(u)= %d, dim(dot_u)= %d\n",
394 m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()),
395 static_cast<int>(dot_u.size()));
396 return -1;
397 }
398 if (type != POSITION_ONLY && type != POSITION_VELOCITY && type != POSITION_VELOCITY_ACCELERATION)
399 {
400 bt_id_error_message("invalid type %d\n", type);
401 return -1;
402 }
403
404 // 1. update relative kinematics
405 // 1.1 for revolute
406 for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
407 {
408 RigidBody &body = m_body_list[m_body_revolute_list[i]];
409 mat33 T;
410 bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &T);
411 body.m_body_T_parent = T * body.m_body_T_parent_ref;
412 if (type >= POSITION_VELOCITY)
413 {
414 body.m_body_ang_vel_rel = body.m_Jac_JR * u(body.m_q_index);
415 }
416 if (type >= POSITION_VELOCITY_ACCELERATION)
417 {
418 body.m_body_ang_acc_rel = body.m_Jac_JR * dot_u(body.m_q_index);
419 }
420 }
421 // 1.2 for prismatic
422 for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
423 {
424 RigidBody &body = m_body_list[m_body_prismatic_list[i]];
425 body.m_parent_pos_parent_body =
426 body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
427 if (type >= POSITION_VELOCITY)
428 {
429 body.m_parent_vel_rel =
430 body.m_body_T_parent_ref.transpose() * body.m_Jac_JT * u(body.m_q_index);
431 }
432 if (type >= POSITION_VELOCITY_ACCELERATION)
433 {
434 body.m_parent_acc_rel = body.m_parent_Jac_JT * dot_u(body.m_q_index);
435 }
436 }
437 // 1.3 fixed joints: nothing to do
438 // 1.4 6dof joints:
439 for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
440 {
441 RigidBody &body = m_body_list[m_body_floating_list[i]];
442
443 body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
444 transformY(q(body.m_q_index + 1)) *
445 transformX(q(body.m_q_index));
446 body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
447 body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
448 body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
449 body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
450
451 if (type >= POSITION_VELOCITY)
452 {
453 body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
454 body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
455 body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
456
457 body.m_parent_vel_rel(0) = u(body.m_q_index + 3);
458 body.m_parent_vel_rel(1) = u(body.m_q_index + 4);
459 body.m_parent_vel_rel(2) = u(body.m_q_index + 5);
460
461 body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
462 }
463 if (type >= POSITION_VELOCITY_ACCELERATION)
464 {
465 body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
466 body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
467 body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
468
469 body.m_parent_acc_rel(0) = dot_u(body.m_q_index + 3);
470 body.m_parent_acc_rel(1) = dot_u(body.m_q_index + 4);
471 body.m_parent_acc_rel(2) = dot_u(body.m_q_index + 5);
472
473 body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
474 }
475 }
476
477 for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
478 {
479 //todo: review
480 RigidBody &body = m_body_list[m_body_spherical_list[i]];
481
482 mat33 T;
483
484 T = transformX(q(body.m_q_index)) *
485 transformY(q(body.m_q_index + 1)) *
486 transformZ(q(body.m_q_index + 2));
487 body.m_body_T_parent = T * body.m_body_T_parent_ref;
488
489 body.m_parent_pos_parent_body(0)=0;
490 body.m_parent_pos_parent_body(1)=0;
491 body.m_parent_pos_parent_body(2)=0;
492
493 body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
494
495 if (type >= POSITION_VELOCITY)
496 {
497 body.m_body_ang_vel_rel(0) = u(body.m_q_index + 0);
498 body.m_body_ang_vel_rel(1) = u(body.m_q_index + 1);
499 body.m_body_ang_vel_rel(2) = u(body.m_q_index + 2);
500 body.m_parent_vel_rel = body.m_body_T_parent.transpose() * body.m_parent_vel_rel;
501 }
502 if (type >= POSITION_VELOCITY_ACCELERATION)
503 {
504 body.m_body_ang_acc_rel(0) = dot_u(body.m_q_index + 0);
505 body.m_body_ang_acc_rel(1) = dot_u(body.m_q_index + 1);
506 body.m_body_ang_acc_rel(2) = dot_u(body.m_q_index + 2);
507 body.m_parent_acc_rel = body.m_body_T_parent.transpose() * body.m_parent_acc_rel;
508 }
509 }
510
511 // 2. absolute kinematic quantities (vector valued)
512 // NOTE: this should be optimized by specializing for different body types
513 // (e.g., relative rotation is always zero for prismatic joints, etc.)
514
515 // calculations for root body
516 {
517 RigidBody &body = m_body_list[0];
518 // 3.1 update absolute positions and orientations:
519 // will be required if we add force elements (eg springs between bodies,
520 // or contacts)
521 // not required right now, added here for debugging purposes
522 body.m_body_pos = body.m_body_T_parent * body.m_parent_pos_parent_body;
523 body.m_body_T_world = body.m_body_T_parent;
524
525 if (type >= POSITION_VELOCITY)
526 {
527 // 3.2 update absolute velocities
528 body.m_body_ang_vel = body.m_body_ang_vel_rel;
529 body.m_body_vel = body.m_parent_vel_rel;
530 }
531 if (type >= POSITION_VELOCITY_ACCELERATION)
532 {
533 // 3.3 update absolute accelerations
534 // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
535 body.m_body_ang_acc = body.m_body_ang_acc_rel;
536 body.m_body_acc = body.m_body_T_parent * body.m_parent_acc_rel;
537 // add gravitational acceleration to root body
538 // this is an efficient way to add gravitational terms,
539 // but it does mean that the kinematics are no longer
540 // correct at the acceleration level
541 // NOTE: To get correct acceleration kinematics, just set world_gravity to zero
542 body.m_body_acc = body.m_body_acc - body.m_body_T_parent * m_world_gravity;
543 }
544 }
545
546 for (idArrayIdx i = 1; i < m_body_list.size(); i++)
547 {
548 RigidBody &body = m_body_list[i];
549 RigidBody &parent = m_body_list[m_parent_index[i]];
550 // 2.1 update absolute positions and orientations:
551 // will be required if we add force elements (eg springs between bodies,
552 // or contacts) not required right now added here for debugging purposes
553 body.m_body_pos =
554 body.m_body_T_parent * (parent.m_body_pos + body.m_parent_pos_parent_body);
555 body.m_body_T_world = body.m_body_T_parent * parent.m_body_T_world;
556
557 if (type >= POSITION_VELOCITY)
558 {
559 // 2.2 update absolute velocities
560 body.m_body_ang_vel =
561 body.m_body_T_parent * parent.m_body_ang_vel + body.m_body_ang_vel_rel;
562
563 body.m_body_vel =
564 body.m_body_T_parent *
565 (parent.m_body_vel + parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body) +
566 body.m_parent_vel_rel);
567 }
568 if (type >= POSITION_VELOCITY_ACCELERATION)
569 {
570 // 2.3 update absolute accelerations
571 // NOTE: assumption: dot(J_JR) = 0; true here, but not for general joints
572 body.m_body_ang_acc =
573 body.m_body_T_parent * parent.m_body_ang_acc -
574 body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel) +
575 body.m_body_ang_acc_rel;
576 body.m_body_acc =
577 body.m_body_T_parent *
578 (parent.m_body_acc + parent.m_body_ang_acc.cross(body.m_parent_pos_parent_body) +
579 parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
580 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel) + body.m_parent_acc_rel);
581 }
582 }
583
584 return 0;
585 }
586
587 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
588
addRelativeJacobianComponent(RigidBody & body)589 void MultiBodyTree::MultiBodyImpl::addRelativeJacobianComponent(RigidBody &body)
590 {
591 const int &idx = body.m_q_index;
592 switch (body.m_joint_type)
593 {
594 case FIXED:
595 break;
596 case REVOLUTE:
597 setMat3xElem(0, idx, body.m_Jac_JR(0), &body.m_body_Jac_R);
598 setMat3xElem(1, idx, body.m_Jac_JR(1), &body.m_body_Jac_R);
599 setMat3xElem(2, idx, body.m_Jac_JR(2), &body.m_body_Jac_R);
600 break;
601 case PRISMATIC:
602 setMat3xElem(0, idx, body.m_body_T_parent_ref(0, 0) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 0) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 0) * body.m_Jac_JT(2),
603 &body.m_body_Jac_T);
604 setMat3xElem(1, idx, body.m_body_T_parent_ref(0, 1) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 1) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 1) * body.m_Jac_JT(2),
605 &body.m_body_Jac_T);
606 setMat3xElem(2, idx, body.m_body_T_parent_ref(0, 2) * body.m_Jac_JT(0) + body.m_body_T_parent_ref(1, 2) * body.m_Jac_JT(1) + body.m_body_T_parent_ref(2, 2) * body.m_Jac_JT(2),
607 &body.m_body_Jac_T);
608 break;
609 case FLOATING:
610 setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
611 setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
612 setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
613 // body_Jac_T = body_T_parent.transpose();
614 setMat3xElem(0, idx + 3, body.m_body_T_parent(0, 0), &body.m_body_Jac_T);
615 setMat3xElem(0, idx + 4, body.m_body_T_parent(1, 0), &body.m_body_Jac_T);
616 setMat3xElem(0, idx + 5, body.m_body_T_parent(2, 0), &body.m_body_Jac_T);
617
618 setMat3xElem(1, idx + 3, body.m_body_T_parent(0, 1), &body.m_body_Jac_T);
619 setMat3xElem(1, idx + 4, body.m_body_T_parent(1, 1), &body.m_body_Jac_T);
620 setMat3xElem(1, idx + 5, body.m_body_T_parent(2, 1), &body.m_body_Jac_T);
621
622 setMat3xElem(2, idx + 3, body.m_body_T_parent(0, 2), &body.m_body_Jac_T);
623 setMat3xElem(2, idx + 4, body.m_body_T_parent(1, 2), &body.m_body_Jac_T);
624 setMat3xElem(2, idx + 5, body.m_body_T_parent(2, 2), &body.m_body_Jac_T);
625
626 break;
627 case SPHERICAL:
628 //todo: review
629 setMat3xElem(0, idx + 0, 1.0, &body.m_body_Jac_R);
630 setMat3xElem(1, idx + 1, 1.0, &body.m_body_Jac_R);
631 setMat3xElem(2, idx + 2, 1.0, &body.m_body_Jac_R);
632 break;
633 }
634 }
635
calculateJacobians(const vecx & q,const vecx & u,const KinUpdateType type)636 int MultiBodyTree::MultiBodyImpl::calculateJacobians(const vecx &q, const vecx &u, const KinUpdateType type)
637 {
638 if (q.size() != m_num_dofs || u.size() != m_num_dofs)
639 {
640 bt_id_error_message(
641 "wrong vector dimension. system has %d DOFs,\n"
642 "but dim(q)= %d, dim(u)= %d\n",
643 m_num_dofs, static_cast<int>(q.size()), static_cast<int>(u.size()));
644 return -1;
645 }
646 if (type != POSITION_ONLY && type != POSITION_VELOCITY)
647 {
648 bt_id_error_message("invalid type %d\n", type);
649 return -1;
650 }
651
652 addRelativeJacobianComponent(m_body_list[0]);
653 for (idArrayIdx i = 1; i < m_body_list.size(); i++)
654 {
655 RigidBody &body = m_body_list[i];
656 RigidBody &parent = m_body_list[m_parent_index[i]];
657
658 mul(body.m_body_T_parent, parent.m_body_Jac_R, &body.m_body_Jac_R);
659 body.m_body_Jac_T = parent.m_body_Jac_T;
660 mul(tildeOperator(body.m_parent_pos_parent_body), parent.m_body_Jac_R, &m_m3x);
661 sub(body.m_body_Jac_T, m_m3x, &body.m_body_Jac_T);
662
663 addRelativeJacobianComponent(body);
664 mul(body.m_body_T_parent, body.m_body_Jac_T, &body.m_body_Jac_T);
665
666 if (type >= POSITION_VELOCITY)
667 {
668 body.m_body_dot_Jac_R_u = body.m_body_T_parent * parent.m_body_dot_Jac_R_u -
669 body.m_body_ang_vel_rel.cross(body.m_body_T_parent * parent.m_body_ang_vel);
670 body.m_body_dot_Jac_T_u = body.m_body_T_parent *
671 (parent.m_body_dot_Jac_T_u + parent.m_body_dot_Jac_R_u.cross(body.m_parent_pos_parent_body) +
672 parent.m_body_ang_vel.cross(parent.m_body_ang_vel.cross(body.m_parent_pos_parent_body)) +
673 2.0 * parent.m_body_ang_vel.cross(body.m_parent_vel_rel));
674 }
675 }
676 return 0;
677 }
678 #endif
679
setThreeDoFJacobians(const int dof,vec3 & Jac_JR,vec3 & Jac_JT)680 static inline void setThreeDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
681 {
682 switch (dof)
683 {
684 // rotational part
685 case 0:
686 Jac_JR(0) = 1;
687 Jac_JR(1) = 0;
688 Jac_JR(2) = 0;
689 setZero(Jac_JT);
690 break;
691 case 1:
692 Jac_JR(0) = 0;
693 Jac_JR(1) = 1;
694 Jac_JR(2) = 0;
695 setZero(Jac_JT);
696 break;
697 case 2:
698 Jac_JR(0) = 0;
699 Jac_JR(1) = 0;
700 Jac_JR(2) = 1;
701 setZero(Jac_JT);
702 break;
703 }
704 }
705
setSixDoFJacobians(const int dof,vec3 & Jac_JR,vec3 & Jac_JT)706 static inline void setSixDoFJacobians(const int dof, vec3 &Jac_JR, vec3 &Jac_JT)
707 {
708 switch (dof)
709 {
710 // rotational part
711 case 0:
712 Jac_JR(0) = 1;
713 Jac_JR(1) = 0;
714 Jac_JR(2) = 0;
715 setZero(Jac_JT);
716 break;
717 case 1:
718 Jac_JR(0) = 0;
719 Jac_JR(1) = 1;
720 Jac_JR(2) = 0;
721 setZero(Jac_JT);
722 break;
723 case 2:
724 Jac_JR(0) = 0;
725 Jac_JR(1) = 0;
726 Jac_JR(2) = 1;
727 setZero(Jac_JT);
728 break;
729 // translational part
730 case 3:
731 setZero(Jac_JR);
732 Jac_JT(0) = 1;
733 Jac_JT(1) = 0;
734 Jac_JT(2) = 0;
735 break;
736 case 4:
737 setZero(Jac_JR);
738 Jac_JT(0) = 0;
739 Jac_JT(1) = 1;
740 Jac_JT(2) = 0;
741 break;
742 case 5:
743 setZero(Jac_JR);
744 Jac_JT(0) = 0;
745 Jac_JT(1) = 0;
746 Jac_JT(2) = 1;
747 break;
748 }
749 }
750
jointNumDoFs(const JointType & type)751 static inline int jointNumDoFs(const JointType &type)
752 {
753 switch (type)
754 {
755 case FIXED:
756 return 0;
757 case REVOLUTE:
758 case PRISMATIC:
759 return 1;
760 case FLOATING:
761 return 6;
762 case SPHERICAL:
763 return 3;
764 }
765 // this should never happen
766 bt_id_error_message("invalid joint type\n");
767 // TODO add configurable abort/crash function
768 abort();
769 return 0;
770 }
771
calculateMassMatrix(const vecx & q,const bool update_kinematics,const bool initialize_matrix,const bool set_lower_triangular_matrix,matxx * mass_matrix)772 int MultiBodyTree::MultiBodyImpl::calculateMassMatrix(const vecx &q, const bool update_kinematics,
773 const bool initialize_matrix,
774 const bool set_lower_triangular_matrix,
775 matxx *mass_matrix)
776 {
777 // This calculates the joint space mass matrix for the multibody system.
778 // The algorithm is essentially an implementation of "method 3"
779 // in "Efficient Dynamic Simulation of Robotic Mechanisms" (Walker and Orin, 1982)
780 // (Later named "Composite Rigid Body Algorithm" by Featherstone).
781 //
782 // This implementation, however, handles branched systems and uses a formulation centered
783 // on the origin of the body-fixed frame to avoid re-computing various quantities at the com.
784
785 if (q.size() != m_num_dofs || mass_matrix->rows() != m_num_dofs ||
786 mass_matrix->cols() != m_num_dofs)
787 {
788 bt_id_error_message(
789 "Dimension error. System has %d DOFs,\n"
790 "but dim(q)= %d, dim(mass_matrix)= %d x %d\n",
791 m_num_dofs, static_cast<int>(q.size()), static_cast<int>(mass_matrix->rows()),
792 static_cast<int>(mass_matrix->cols()));
793 return -1;
794 }
795
796 // TODO add optimized zeroing function?
797 if (initialize_matrix)
798 {
799 for (int i = 0; i < m_num_dofs; i++)
800 {
801 for (int j = 0; j < m_num_dofs; j++)
802 {
803 setMatxxElem(i, j, 0.0, mass_matrix);
804 }
805 }
806 }
807
808 if (update_kinematics)
809 {
810 // 1. update relative kinematics
811 // 1.1 for revolute joints
812 for (idArrayIdx i = 0; i < m_body_revolute_list.size(); i++)
813 {
814 RigidBody &body = m_body_list[m_body_revolute_list[i]];
815 // from reference orientation (q=0) of body-fixed frame to current orientation
816 mat33 body_T_body_ref;
817 bodyTParentFromAxisAngle(body.m_Jac_JR, q(body.m_q_index), &body_T_body_ref);
818 body.m_body_T_parent = body_T_body_ref * body.m_body_T_parent_ref;
819 }
820 // 1.2 for prismatic joints
821 for (idArrayIdx i = 0; i < m_body_prismatic_list.size(); i++)
822 {
823 RigidBody &body = m_body_list[m_body_prismatic_list[i]];
824 // body.m_body_T_parent= fixed
825 body.m_parent_pos_parent_body =
826 body.m_parent_pos_parent_body_ref + body.m_parent_Jac_JT * q(body.m_q_index);
827 }
828 // 1.3 fixed joints: nothing to do
829 // 1.4 6dof joints:
830 for (idArrayIdx i = 0; i < m_body_floating_list.size(); i++)
831 {
832 RigidBody &body = m_body_list[m_body_floating_list[i]];
833
834 body.m_body_T_parent = transformZ(q(body.m_q_index + 2)) *
835 transformY(q(body.m_q_index + 1)) *
836 transformX(q(body.m_q_index));
837 body.m_parent_pos_parent_body(0) = q(body.m_q_index + 3);
838 body.m_parent_pos_parent_body(1) = q(body.m_q_index + 4);
839 body.m_parent_pos_parent_body(2) = q(body.m_q_index + 5);
840
841 body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
842 }
843
844 for (idArrayIdx i = 0; i < m_body_spherical_list.size(); i++)
845 {
846 //todo: review
847 RigidBody &body = m_body_list[m_body_spherical_list[i]];
848
849 mat33 T;
850
851 T = transformX(q(body.m_q_index)) *
852 transformY(q(body.m_q_index + 1)) *
853 transformZ(q(body.m_q_index + 2));
854 body.m_body_T_parent = T * body.m_body_T_parent_ref;
855
856 body.m_parent_pos_parent_body(0)=0;
857 body.m_parent_pos_parent_body(1)=0;
858 body.m_parent_pos_parent_body(2)=0;
859
860 body.m_parent_pos_parent_body = body.m_body_T_parent * body.m_parent_pos_parent_body;
861 }
862 }
863 for (int i = m_body_list.size() - 1; i >= 0; i--)
864 {
865 RigidBody &body = m_body_list[i];
866 // calculate mass, center of mass and inertia of "composite rigid body",
867 // ie, sub-tree starting at current body
868 body.m_subtree_mass = body.m_mass;
869 body.m_body_subtree_mass_com = body.m_body_mass_com;
870 body.m_body_subtree_I_body = body.m_body_I_body;
871
872 for (idArrayIdx c = 0; c < m_child_indices[i].size(); c++)
873 {
874 RigidBody &child = m_body_list[m_child_indices[i][c]];
875 mat33 body_T_child = child.m_body_T_parent.transpose();
876
877 body.m_subtree_mass += child.m_subtree_mass;
878 body.m_body_subtree_mass_com += body_T_child * child.m_body_subtree_mass_com +
879 child.m_parent_pos_parent_body * child.m_subtree_mass;
880 body.m_body_subtree_I_body +=
881 body_T_child * child.m_body_subtree_I_body * child.m_body_T_parent;
882
883 if (child.m_subtree_mass > 0)
884 {
885 // Shift the reference point for the child subtree inertia using the
886 // Huygens-Steiner ("parallel axis") theorem.
887 // (First shift from child origin to child com, then from there to this body's
888 // origin)
889 vec3 r_com = body_T_child * child.m_body_subtree_mass_com / child.m_subtree_mass;
890 mat33 tilde_r_child_com = tildeOperator(r_com);
891 mat33 tilde_r_body_com = tildeOperator(child.m_parent_pos_parent_body + r_com);
892 body.m_body_subtree_I_body +=
893 child.m_subtree_mass *
894 (tilde_r_child_com * tilde_r_child_com - tilde_r_body_com * tilde_r_body_com);
895 }
896 }
897 }
898
899 for (int i = m_body_list.size() - 1; i >= 0; i--)
900 {
901 const RigidBody &body = m_body_list[i];
902
903 // determine DoF-range for body
904 const int q_index_min = body.m_q_index;
905 const int q_index_max = q_index_min + jointNumDoFs(body.m_joint_type) - 1;
906 // loop over the DoFs used by this body
907 // local joint jacobians (ok as is for 1-DoF joints)
908 vec3 Jac_JR = body.m_Jac_JR;
909 vec3 Jac_JT = body.m_Jac_JT;
910 for (int col = q_index_max; col >= q_index_min; col--)
911 {
912 // set jacobians for 6-DoF joints
913 if (FLOATING == body.m_joint_type)
914 {
915 setSixDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
916 }
917 if (SPHERICAL == body.m_joint_type)
918 {
919 //todo: review
920 setThreeDoFJacobians(col - q_index_min, Jac_JR, Jac_JT);
921 }
922
923 vec3 body_eom_rot =
924 body.m_body_subtree_I_body * Jac_JR + body.m_body_subtree_mass_com.cross(Jac_JT);
925 vec3 body_eom_trans =
926 body.m_subtree_mass * Jac_JT - body.m_body_subtree_mass_com.cross(Jac_JR);
927 setMatxxElem(col, col, Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans), mass_matrix);
928
929 // rest of the mass matrix column upwards
930 {
931 // 1. for multi-dof joints, rest of the dofs of this body
932 for (int row = col - 1; row >= q_index_min; row--)
933 {
934 if (SPHERICAL == body.m_joint_type)
935 {
936 //todo: review
937 setThreeDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
938 const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
939 setMatxxElem(col, row, Mrc, mass_matrix);
940 }
941 if (FLOATING == body.m_joint_type)
942 {
943 setSixDoFJacobians(row - q_index_min, Jac_JR, Jac_JT);
944 const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
945 setMatxxElem(col, row, Mrc, mass_matrix);
946 }
947 }
948 // 2. ancestor dofs
949 int child_idx = i;
950 int parent_idx = m_parent_index[i];
951 while (parent_idx >= 0)
952 {
953 const RigidBody &child_body = m_body_list[child_idx];
954 const RigidBody &parent_body = m_body_list[parent_idx];
955
956 const mat33 parent_T_child = child_body.m_body_T_parent.transpose();
957 body_eom_rot = parent_T_child * body_eom_rot;
958 body_eom_trans = parent_T_child * body_eom_trans;
959 body_eom_rot += child_body.m_parent_pos_parent_body.cross(body_eom_trans);
960
961 const int parent_body_q_index_min = parent_body.m_q_index;
962 const int parent_body_q_index_max =
963 parent_body_q_index_min + jointNumDoFs(parent_body.m_joint_type) - 1;
964 vec3 Jac_JR = parent_body.m_Jac_JR;
965 vec3 Jac_JT = parent_body.m_Jac_JT;
966 for (int row = parent_body_q_index_max; row >= parent_body_q_index_min; row--)
967 {
968 if (SPHERICAL == parent_body.m_joint_type)
969 {
970 //todo: review
971 setThreeDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
972 }
973 // set jacobians for 6-DoF joints
974 if (FLOATING == parent_body.m_joint_type)
975 {
976 setSixDoFJacobians(row - parent_body_q_index_min, Jac_JR, Jac_JT);
977 }
978 const double Mrc = Jac_JR.dot(body_eom_rot) + Jac_JT.dot(body_eom_trans);
979 setMatxxElem(col, row, Mrc, mass_matrix);
980 }
981
982 child_idx = parent_idx;
983 parent_idx = m_parent_index[child_idx];
984 }
985 }
986 }
987 }
988
989 if (set_lower_triangular_matrix)
990 {
991 for (int col = 0; col < m_num_dofs; col++)
992 {
993 for (int row = 0; row < col; row++)
994 {
995 setMatxxElem(row, col, (*mass_matrix)(col, row), mass_matrix);
996 }
997 }
998 }
999 return 0;
1000 }
1001
1002 // utility macro
1003 #define CHECK_IF_BODY_INDEX_IS_VALID(index) \
1004 do \
1005 { \
1006 if (index < 0 || index >= m_num_bodies) \
1007 { \
1008 bt_id_error_message("invalid index %d (num_bodies= %d)\n", index, m_num_bodies); \
1009 return -1; \
1010 } \
1011 } while (0)
1012
getParentIndex(const int body_index,int * p)1013 int MultiBodyTree::MultiBodyImpl::getParentIndex(const int body_index, int *p)
1014 {
1015 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1016 *p = m_parent_index[body_index];
1017 return 0;
1018 }
1019
getUserInt(const int body_index,int * user_int) const1020 int MultiBodyTree::MultiBodyImpl::getUserInt(const int body_index, int *user_int) const
1021 {
1022 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1023 *user_int = m_user_int[body_index];
1024 return 0;
1025 }
getUserPtr(const int body_index,void ** user_ptr) const1026 int MultiBodyTree::MultiBodyImpl::getUserPtr(const int body_index, void **user_ptr) const
1027 {
1028 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1029 *user_ptr = m_user_ptr[body_index];
1030 return 0;
1031 }
1032
setUserInt(const int body_index,const int user_int)1033 int MultiBodyTree::MultiBodyImpl::setUserInt(const int body_index, const int user_int)
1034 {
1035 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1036 m_user_int[body_index] = user_int;
1037 return 0;
1038 }
1039
setUserPtr(const int body_index,void * const user_ptr)1040 int MultiBodyTree::MultiBodyImpl::setUserPtr(const int body_index, void *const user_ptr)
1041 {
1042 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1043 m_user_ptr[body_index] = user_ptr;
1044 return 0;
1045 }
1046
getBodyOrigin(int body_index,vec3 * world_origin) const1047 int MultiBodyTree::MultiBodyImpl::getBodyOrigin(int body_index, vec3 *world_origin) const
1048 {
1049 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1050 const RigidBody &body = m_body_list[body_index];
1051 *world_origin = body.m_body_T_world.transpose() * body.m_body_pos;
1052 return 0;
1053 }
1054
getBodyCoM(int body_index,vec3 * world_com) const1055 int MultiBodyTree::MultiBodyImpl::getBodyCoM(int body_index, vec3 *world_com) const
1056 {
1057 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1058 const RigidBody &body = m_body_list[body_index];
1059 if (body.m_mass > 0)
1060 {
1061 *world_com = body.m_body_T_world.transpose() *
1062 (body.m_body_pos + body.m_body_mass_com / body.m_mass);
1063 }
1064 else
1065 {
1066 *world_com = body.m_body_T_world.transpose() * (body.m_body_pos);
1067 }
1068 return 0;
1069 }
1070
getBodyTransform(int body_index,mat33 * world_T_body) const1071 int MultiBodyTree::MultiBodyImpl::getBodyTransform(int body_index, mat33 *world_T_body) const
1072 {
1073 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1074 const RigidBody &body = m_body_list[body_index];
1075 *world_T_body = body.m_body_T_world.transpose();
1076 return 0;
1077 }
getBodyAngularVelocity(int body_index,vec3 * world_omega) const1078 int MultiBodyTree::MultiBodyImpl::getBodyAngularVelocity(int body_index, vec3 *world_omega) const
1079 {
1080 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1081 const RigidBody &body = m_body_list[body_index];
1082 *world_omega = body.m_body_T_world.transpose() * body.m_body_ang_vel;
1083 return 0;
1084 }
getBodyLinearVelocity(int body_index,vec3 * world_velocity) const1085 int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocity(int body_index,
1086 vec3 *world_velocity) const
1087 {
1088 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1089 const RigidBody &body = m_body_list[body_index];
1090 *world_velocity = body.m_body_T_world.transpose() * body.m_body_vel;
1091 return 0;
1092 }
1093
getBodyLinearVelocityCoM(int body_index,vec3 * world_velocity) const1094 int MultiBodyTree::MultiBodyImpl::getBodyLinearVelocityCoM(int body_index,
1095 vec3 *world_velocity) const
1096 {
1097 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1098 const RigidBody &body = m_body_list[body_index];
1099 vec3 com;
1100 if (body.m_mass > 0)
1101 {
1102 com = body.m_body_mass_com / body.m_mass;
1103 }
1104 else
1105 {
1106 com(0) = 0;
1107 com(1) = 0;
1108 com(2) = 0;
1109 }
1110
1111 *world_velocity =
1112 body.m_body_T_world.transpose() * (body.m_body_vel + body.m_body_ang_vel.cross(com));
1113 return 0;
1114 }
1115
getBodyAngularAcceleration(int body_index,vec3 * world_dot_omega) const1116 int MultiBodyTree::MultiBodyImpl::getBodyAngularAcceleration(int body_index,
1117 vec3 *world_dot_omega) const
1118 {
1119 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1120 const RigidBody &body = m_body_list[body_index];
1121 *world_dot_omega = body.m_body_T_world.transpose() * body.m_body_ang_acc;
1122 return 0;
1123 }
getBodyLinearAcceleration(int body_index,vec3 * world_acceleration) const1124 int MultiBodyTree::MultiBodyImpl::getBodyLinearAcceleration(int body_index,
1125 vec3 *world_acceleration) const
1126 {
1127 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1128 const RigidBody &body = m_body_list[body_index];
1129 *world_acceleration = body.m_body_T_world.transpose() * body.m_body_acc;
1130 return 0;
1131 }
1132
getJointType(const int body_index,JointType * joint_type) const1133 int MultiBodyTree::MultiBodyImpl::getJointType(const int body_index, JointType *joint_type) const
1134 {
1135 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1136 *joint_type = m_body_list[body_index].m_joint_type;
1137 return 0;
1138 }
1139
getJointTypeStr(const int body_index,const char ** joint_type) const1140 int MultiBodyTree::MultiBodyImpl::getJointTypeStr(const int body_index,
1141 const char **joint_type) const
1142 {
1143 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1144 *joint_type = jointTypeToString(m_body_list[body_index].m_joint_type);
1145 return 0;
1146 }
1147
getParentRParentBodyRef(const int body_index,vec3 * r) const1148 int MultiBodyTree::MultiBodyImpl::getParentRParentBodyRef(const int body_index, vec3 *r) const
1149 {
1150 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1151 *r = m_body_list[body_index].m_parent_pos_parent_body_ref;
1152 return 0;
1153 }
1154
getBodyTParentRef(const int body_index,mat33 * T) const1155 int MultiBodyTree::MultiBodyImpl::getBodyTParentRef(const int body_index, mat33 *T) const
1156 {
1157 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1158 *T = m_body_list[body_index].m_body_T_parent_ref;
1159 return 0;
1160 }
1161
getBodyAxisOfMotion(const int body_index,vec3 * axis) const1162 int MultiBodyTree::MultiBodyImpl::getBodyAxisOfMotion(const int body_index, vec3 *axis) const
1163 {
1164 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1165 if (m_body_list[body_index].m_joint_type == REVOLUTE)
1166 {
1167 *axis = m_body_list[body_index].m_Jac_JR;
1168 return 0;
1169 }
1170 if (m_body_list[body_index].m_joint_type == PRISMATIC)
1171 {
1172 *axis = m_body_list[body_index].m_Jac_JT;
1173 return 0;
1174 }
1175 setZero(*axis);
1176 return 0;
1177 }
1178
getDoFOffset(const int body_index,int * q_index) const1179 int MultiBodyTree::MultiBodyImpl::getDoFOffset(const int body_index, int *q_index) const
1180 {
1181 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1182 *q_index = m_body_list[body_index].m_q_index;
1183 return 0;
1184 }
1185
setBodyMass(const int body_index,const idScalar mass)1186 int MultiBodyTree::MultiBodyImpl::setBodyMass(const int body_index, const idScalar mass)
1187 {
1188 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1189 m_body_list[body_index].m_mass = mass;
1190 return 0;
1191 }
1192
setBodyFirstMassMoment(const int body_index,const vec3 & first_mass_moment)1193 int MultiBodyTree::MultiBodyImpl::setBodyFirstMassMoment(const int body_index,
1194 const vec3 &first_mass_moment)
1195 {
1196 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1197 m_body_list[body_index].m_body_mass_com = first_mass_moment;
1198 return 0;
1199 }
setBodySecondMassMoment(const int body_index,const mat33 & second_mass_moment)1200 int MultiBodyTree::MultiBodyImpl::setBodySecondMassMoment(const int body_index,
1201 const mat33 &second_mass_moment)
1202 {
1203 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1204 m_body_list[body_index].m_body_I_body = second_mass_moment;
1205 return 0;
1206 }
getBodyMass(const int body_index,idScalar * mass) const1207 int MultiBodyTree::MultiBodyImpl::getBodyMass(const int body_index, idScalar *mass) const
1208 {
1209 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1210 *mass = m_body_list[body_index].m_mass;
1211 return 0;
1212 }
getBodyFirstMassMoment(const int body_index,vec3 * first_mass_moment) const1213 int MultiBodyTree::MultiBodyImpl::getBodyFirstMassMoment(const int body_index,
1214 vec3 *first_mass_moment) const
1215 {
1216 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1217 *first_mass_moment = m_body_list[body_index].m_body_mass_com;
1218 return 0;
1219 }
getBodySecondMassMoment(const int body_index,mat33 * second_mass_moment) const1220 int MultiBodyTree::MultiBodyImpl::getBodySecondMassMoment(const int body_index,
1221 mat33 *second_mass_moment) const
1222 {
1223 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1224 *second_mass_moment = m_body_list[body_index].m_body_I_body;
1225 return 0;
1226 }
1227
clearAllUserForcesAndMoments()1228 void MultiBodyTree::MultiBodyImpl::clearAllUserForcesAndMoments()
1229 {
1230 for (int index = 0; index < m_num_bodies; index++)
1231 {
1232 RigidBody &body = m_body_list[index];
1233 setZero(body.m_body_force_user);
1234 setZero(body.m_body_moment_user);
1235 }
1236 }
1237
addUserForce(const int body_index,const vec3 & body_force)1238 int MultiBodyTree::MultiBodyImpl::addUserForce(const int body_index, const vec3 &body_force)
1239 {
1240 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1241 m_body_list[body_index].m_body_force_user += body_force;
1242 return 0;
1243 }
1244
addUserMoment(const int body_index,const vec3 & body_moment)1245 int MultiBodyTree::MultiBodyImpl::addUserMoment(const int body_index, const vec3 &body_moment)
1246 {
1247 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1248 m_body_list[body_index].m_body_moment_user += body_moment;
1249 return 0;
1250 }
1251
1252 #if (defined BT_ID_HAVE_MAT3X) && (defined BT_ID_WITH_JACOBIANS)
getBodyDotJacobianTransU(const int body_index,vec3 * world_dot_jac_trans_u) const1253 int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianTransU(const int body_index, vec3 *world_dot_jac_trans_u) const
1254 {
1255 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1256 const RigidBody &body = m_body_list[body_index];
1257 *world_dot_jac_trans_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_T_u;
1258 return 0;
1259 }
1260
getBodyDotJacobianRotU(const int body_index,vec3 * world_dot_jac_rot_u) const1261 int MultiBodyTree::MultiBodyImpl::getBodyDotJacobianRotU(const int body_index, vec3 *world_dot_jac_rot_u) const
1262 {
1263 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1264 const RigidBody &body = m_body_list[body_index];
1265 *world_dot_jac_rot_u = body.m_body_T_world.transpose() * body.m_body_dot_Jac_R_u;
1266 return 0;
1267 }
1268
getBodyJacobianTrans(const int body_index,mat3x * world_jac_trans) const1269 int MultiBodyTree::MultiBodyImpl::getBodyJacobianTrans(const int body_index, mat3x *world_jac_trans) const
1270 {
1271 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1272 const RigidBody &body = m_body_list[body_index];
1273 mul(body.m_body_T_world.transpose(), body.m_body_Jac_T, world_jac_trans);
1274 return 0;
1275 }
1276
getBodyJacobianRot(const int body_index,mat3x * world_jac_rot) const1277 int MultiBodyTree::MultiBodyImpl::getBodyJacobianRot(const int body_index, mat3x *world_jac_rot) const
1278 {
1279 CHECK_IF_BODY_INDEX_IS_VALID(body_index);
1280 const RigidBody &body = m_body_list[body_index];
1281 mul(body.m_body_T_world.transpose(), body.m_body_Jac_R, world_jac_rot);
1282 return 0;
1283 }
1284
1285 #endif
1286 } // namespace btInverseDynamics
1287