1 /*
2 * PURPOSE:
3 * Class representing an articulated rigid body. Stores the body's
4 * current state, allows forces and torques to be set, handles
5 * timestepping and implements Featherstone's algorithm.
6 *
7 * COPYRIGHT:
8 * Copyright (C) Stephen Thompson, <stephen@solarflare.org.uk>, 2011-2013
9 * Portions written By Erwin Coumans: replacing Eigen math library by Bullet LinearMath and a dedicated 6x6 matrix inverse (solveImatrix)
10
11 This software is provided 'as-is', without any express or implied warranty.
12 In no event will the authors be held liable for any damages arising from the use of this software.
13 Permission is granted to anyone to use this software for any purpose,
14 including commercial applications, and to alter it and redistribute it freely,
15 subject to the following restrictions:
16
17 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
18 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
19 3. This notice may not be removed or altered from any source distribution.
20
21 */
22
23
24 #include "btMultiBody.h"
25 #include "btMultiBodyLink.h"
26 #include "btMultiBodyLinkCollider.h"
27 #include "LinearMath/btTransformUtil.h"
28
29 // #define INCLUDE_GYRO_TERM
30
31 namespace {
32 const btScalar SLEEP_EPSILON = btScalar(0.05); // this is a squared velocity (m^2 s^-2)
33 const btScalar SLEEP_TIMEOUT = btScalar(2); // in seconds
34 }
35
36 namespace {
SpatialTransform(const btMatrix3x3 & rotation_matrix,const btVector3 & displacement,const btVector3 & top_in,const btVector3 & bottom_in,btVector3 & top_out,btVector3 & bottom_out)37 void SpatialTransform(const btMatrix3x3 &rotation_matrix, // rotates vectors in 'from' frame to vectors in 'to' frame
38 const btVector3 &displacement, // vector from origin of 'from' frame to origin of 'to' frame, in 'to' coordinates
39 const btVector3 &top_in, // top part of input vector
40 const btVector3 &bottom_in, // bottom part of input vector
41 btVector3 &top_out, // top part of output vector
42 btVector3 &bottom_out) // bottom part of output vector
43 {
44 top_out = rotation_matrix * top_in;
45 bottom_out = -displacement.cross(top_out) + rotation_matrix * bottom_in;
46 }
47
InverseSpatialTransform(const btMatrix3x3 & rotation_matrix,const btVector3 & displacement,const btVector3 & top_in,const btVector3 & bottom_in,btVector3 & top_out,btVector3 & bottom_out)48 void InverseSpatialTransform(const btMatrix3x3 &rotation_matrix,
49 const btVector3 &displacement,
50 const btVector3 &top_in,
51 const btVector3 &bottom_in,
52 btVector3 &top_out,
53 btVector3 &bottom_out)
54 {
55 top_out = rotation_matrix.transpose() * top_in;
56 bottom_out = rotation_matrix.transpose() * (bottom_in + displacement.cross(top_in));
57 }
58
SpatialDotProduct(const btVector3 & a_top,const btVector3 & a_bottom,const btVector3 & b_top,const btVector3 & b_bottom)59 btScalar SpatialDotProduct(const btVector3 &a_top,
60 const btVector3 &a_bottom,
61 const btVector3 &b_top,
62 const btVector3 &b_bottom)
63 {
64 return a_bottom.dot(b_top) + a_top.dot(b_bottom);
65 }
66
SpatialCrossProduct(const btVector3 & a_top,const btVector3 & a_bottom,const btVector3 & b_top,const btVector3 & b_bottom,btVector3 & top_out,btVector3 & bottom_out)67 void SpatialCrossProduct(const btVector3 &a_top,
68 const btVector3 &a_bottom,
69 const btVector3 &b_top,
70 const btVector3 &b_bottom,
71 btVector3 &top_out,
72 btVector3 &bottom_out)
73 {
74 top_out = a_top.cross(b_top);
75 bottom_out = a_bottom.cross(b_top) + a_top.cross(b_bottom);
76 }
77 }
78
79
80 //
81 // Implementation of class btMultiBody
82 //
83
btMultiBody(int n_links,btScalar mass,const btVector3 & inertia,bool fixedBase,bool canSleep,bool multiDof)84 btMultiBody::btMultiBody(int n_links,
85 btScalar mass,
86 const btVector3 &inertia,
87 bool fixedBase,
88 bool canSleep,
89 bool multiDof)
90 :
91 m_baseCollider(0),
92 m_basePos(0,0,0),
93 m_baseQuat(0, 0, 0, 1),
94 m_baseMass(mass),
95 m_baseInertia(inertia),
96
97 m_fixedBase(fixedBase),
98 m_awake(true),
99 m_canSleep(canSleep),
100 m_sleepTimer(0),
101
102 m_linearDamping(0.04f),
103 m_angularDamping(0.04f),
104 m_useGyroTerm(true),
105 m_maxAppliedImpulse(1000.f),
106 m_maxCoordinateVelocity(100.f),
107 m_hasSelfCollision(true),
108 m_isMultiDof(multiDof),
109 __posUpdated(false),
110 m_dofCount(0),
111 m_posVarCnt(0),
112 m_useRK4(false),
113 m_useGlobalVelocities(false)
114 {
115
116 if(!m_isMultiDof)
117 {
118 m_vectorBuf.resize(2*n_links);
119 m_realBuf.resize(6 + 2*n_links);
120 m_posVarCnt = n_links;
121 }
122
123 m_links.resize(n_links);
124 m_matrixBuf.resize(n_links + 1);
125
126
127 m_baseForce.setValue(0, 0, 0);
128 m_baseTorque.setValue(0, 0, 0);
129 }
130
~btMultiBody()131 btMultiBody::~btMultiBody()
132 {
133 }
134
setupFixed(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)135 void btMultiBody::setupFixed(int i,
136 btScalar mass,
137 const btVector3 &inertia,
138 int parent,
139 const btQuaternion &rotParentToThis,
140 const btVector3 &parentComToThisPivotOffset,
141 const btVector3 &thisPivotToThisComOffset,
142 bool disableParentCollision)
143 {
144 btAssert(m_isMultiDof);
145
146 m_links[i].m_mass = mass;
147 m_links[i].m_inertiaLocal = inertia;
148 m_links[i].m_parent = parent;
149 m_links[i].m_zeroRotParentToThis = rotParentToThis;
150 m_links[i].m_dVector = thisPivotToThisComOffset;
151 m_links[i].m_eVector = parentComToThisPivotOffset;
152
153 m_links[i].m_jointType = btMultibodyLink::eFixed;
154 m_links[i].m_dofCount = 0;
155 m_links[i].m_posVarCount = 0;
156
157 if (disableParentCollision)
158 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
159 //
160 m_links[i].updateCacheMultiDof();
161
162 //
163 //if(m_isMultiDof)
164 // resizeInternalMultiDofBuffers();
165 //
166 updateLinksDofOffsets();
167
168 }
169
170
setupPrismatic(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & jointAxis,const btVector3 & parentComToThisComOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)171 void btMultiBody::setupPrismatic(int i,
172 btScalar mass,
173 const btVector3 &inertia,
174 int parent,
175 const btQuaternion &rotParentToThis,
176 const btVector3 &jointAxis,
177 const btVector3 &parentComToThisComOffset,
178 const btVector3 &thisPivotToThisComOffset,
179 bool disableParentCollision)
180 {
181 if(m_isMultiDof)
182 {
183 m_dofCount += 1;
184 m_posVarCnt += 1;
185 }
186
187 m_links[i].m_mass = mass;
188 m_links[i].m_inertiaLocal = inertia;
189 m_links[i].m_parent = parent;
190 m_links[i].m_zeroRotParentToThis = rotParentToThis;
191 m_links[i].setAxisTop(0, 0., 0., 0.);
192 m_links[i].setAxisBottom(0, jointAxis);
193 m_links[i].m_eVector = parentComToThisComOffset;
194 m_links[i].m_dVector = thisPivotToThisComOffset;
195 m_links[i].m_cachedRotParentToThis = rotParentToThis;
196
197 m_links[i].m_jointType = btMultibodyLink::ePrismatic;
198 m_links[i].m_dofCount = 1;
199 m_links[i].m_posVarCount = 1;
200 m_links[i].m_jointPos[0] = 0.f;
201 m_links[i].m_jointTorque[0] = 0.f;
202
203 if (disableParentCollision)
204 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
205 //
206 if(m_isMultiDof)
207 m_links[i].updateCacheMultiDof();
208 else
209 m_links[i].updateCache();
210 //
211 if(m_isMultiDof)
212 updateLinksDofOffsets();
213 }
214
setupRevolute(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & jointAxis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)215 void btMultiBody::setupRevolute(int i,
216 btScalar mass,
217 const btVector3 &inertia,
218 int parent,
219 const btQuaternion &rotParentToThis,
220 const btVector3 &jointAxis,
221 const btVector3 &parentComToThisPivotOffset,
222 const btVector3 &thisPivotToThisComOffset,
223 bool disableParentCollision)
224 {
225 if(m_isMultiDof)
226 {
227 m_dofCount += 1;
228 m_posVarCnt += 1;
229 }
230
231 m_links[i].m_mass = mass;
232 m_links[i].m_inertiaLocal = inertia;
233 m_links[i].m_parent = parent;
234 m_links[i].m_zeroRotParentToThis = rotParentToThis;
235 m_links[i].setAxisTop(0, jointAxis);
236 m_links[i].setAxisBottom(0, jointAxis.cross(thisPivotToThisComOffset));
237 m_links[i].m_dVector = thisPivotToThisComOffset;
238 m_links[i].m_eVector = parentComToThisPivotOffset;
239
240 m_links[i].m_jointType = btMultibodyLink::eRevolute;
241 m_links[i].m_dofCount = 1;
242 m_links[i].m_posVarCount = 1;
243 m_links[i].m_jointPos[0] = 0.f;
244 m_links[i].m_jointTorque[0] = 0.f;
245
246 if (disableParentCollision)
247 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
248 //
249 if(m_isMultiDof)
250 m_links[i].updateCacheMultiDof();
251 else
252 m_links[i].updateCache();
253 //
254 if(m_isMultiDof)
255 updateLinksDofOffsets();
256 }
257
258
259
setupSpherical(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & parentComToThisPivotOffset,const btVector3 & thisPivotToThisComOffset,bool disableParentCollision)260 void btMultiBody::setupSpherical(int i,
261 btScalar mass,
262 const btVector3 &inertia,
263 int parent,
264 const btQuaternion &rotParentToThis,
265 const btVector3 &parentComToThisPivotOffset,
266 const btVector3 &thisPivotToThisComOffset,
267 bool disableParentCollision)
268 {
269 btAssert(m_isMultiDof);
270 m_dofCount += 3;
271 m_posVarCnt += 4;
272
273 m_links[i].m_mass = mass;
274 m_links[i].m_inertiaLocal = inertia;
275 m_links[i].m_parent = parent;
276 m_links[i].m_zeroRotParentToThis = rotParentToThis;
277 m_links[i].m_dVector = thisPivotToThisComOffset;
278 m_links[i].m_eVector = parentComToThisPivotOffset;
279
280 m_links[i].m_jointType = btMultibodyLink::eSpherical;
281 m_links[i].m_dofCount = 3;
282 m_links[i].m_posVarCount = 4;
283 m_links[i].setAxisTop(0, 1.f, 0.f, 0.f);
284 m_links[i].setAxisTop(1, 0.f, 1.f, 0.f);
285 m_links[i].setAxisTop(2, 0.f, 0.f, 1.f);
286 m_links[i].setAxisBottom(0, m_links[i].getAxisTop(0).cross(thisPivotToThisComOffset));
287 m_links[i].setAxisBottom(1, m_links[i].getAxisTop(1).cross(thisPivotToThisComOffset));
288 m_links[i].setAxisBottom(2, m_links[i].getAxisTop(2).cross(thisPivotToThisComOffset));
289 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f; m_links[i].m_jointPos[3] = 1.f;
290 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
291
292
293 if (disableParentCollision)
294 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
295 //
296 m_links[i].updateCacheMultiDof();
297 //
298 updateLinksDofOffsets();
299 }
300
301 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
setupPlanar(int i,btScalar mass,const btVector3 & inertia,int parent,const btQuaternion & rotParentToThis,const btVector3 & rotationAxis,const btVector3 & parentComToThisComOffset,bool disableParentCollision)302 void btMultiBody::setupPlanar(int i,
303 btScalar mass,
304 const btVector3 &inertia,
305 int parent,
306 const btQuaternion &rotParentToThis,
307 const btVector3 &rotationAxis,
308 const btVector3 &parentComToThisComOffset,
309 bool disableParentCollision)
310 {
311 btAssert(m_isMultiDof);
312 m_dofCount += 3;
313 m_posVarCnt += 3;
314
315 m_links[i].m_mass = mass;
316 m_links[i].m_inertiaLocal = inertia;
317 m_links[i].m_parent = parent;
318 m_links[i].m_zeroRotParentToThis = rotParentToThis;
319 m_links[i].m_dVector.setZero();
320 m_links[i].m_eVector = parentComToThisComOffset;
321
322 //
323 static btVector3 vecNonParallelToRotAxis(1, 0, 0);
324 if(rotationAxis.normalized().dot(vecNonParallelToRotAxis) > 0.999)
325 vecNonParallelToRotAxis.setValue(0, 1, 0);
326 //
327
328 m_links[i].m_jointType = btMultibodyLink::ePlanar;
329 m_links[i].m_dofCount = 3;
330 m_links[i].m_posVarCount = 3;
331 btVector3 n=rotationAxis.normalized();
332 m_links[i].setAxisTop(0, n[0],n[1],n[2]);
333 m_links[i].setAxisTop(1,0,0,0);
334 m_links[i].setAxisTop(2,0,0,0);
335 m_links[i].setAxisBottom(0,0,0,0);
336 btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
337 m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
338 cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
339 m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
340 m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
341 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
342
343 if (disableParentCollision)
344 m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
345 //
346 m_links[i].updateCacheMultiDof();
347 //
348 updateLinksDofOffsets();
349 }
350 #endif
351
finalizeMultiDof()352 void btMultiBody::finalizeMultiDof()
353 {
354 btAssert(m_isMultiDof);
355
356 m_realBuf.resize(6 + m_dofCount + m_dofCount*m_dofCount + 6 + m_dofCount); //m_dofCount for joint-space vels + m_dofCount^2 for "D" matrices + delta-pos vector (6 base "vels" + joint "vels")
357 m_vectorBuf.resize(2 * m_dofCount); //two 3-vectors (i.e. one six-vector) for each system dof ("h" matrices)
358
359 updateLinksDofOffsets();
360 }
361
getParent(int i) const362 int btMultiBody::getParent(int i) const
363 {
364 return m_links[i].m_parent;
365 }
366
getLinkMass(int i) const367 btScalar btMultiBody::getLinkMass(int i) const
368 {
369 return m_links[i].m_mass;
370 }
371
getLinkInertia(int i) const372 const btVector3 & btMultiBody::getLinkInertia(int i) const
373 {
374 return m_links[i].m_inertiaLocal;
375 }
376
getJointPos(int i) const377 btScalar btMultiBody::getJointPos(int i) const
378 {
379 return m_links[i].m_jointPos[0];
380 }
381
getJointVel(int i) const382 btScalar btMultiBody::getJointVel(int i) const
383 {
384 return m_realBuf[6 + i];
385 }
386
getJointPosMultiDof(int i)387 btScalar * btMultiBody::getJointPosMultiDof(int i)
388 {
389 return &m_links[i].m_jointPos[0];
390 }
391
getJointVelMultiDof(int i)392 btScalar * btMultiBody::getJointVelMultiDof(int i)
393 {
394 return &m_realBuf[6 + m_links[i].m_dofOffset];
395 }
396
setJointPos(int i,btScalar q)397 void btMultiBody::setJointPos(int i, btScalar q)
398 {
399 m_links[i].m_jointPos[0] = q;
400 m_links[i].updateCache();
401 }
402
setJointPosMultiDof(int i,btScalar * q)403 void btMultiBody::setJointPosMultiDof(int i, btScalar *q)
404 {
405 for(int pos = 0; pos < m_links[i].m_posVarCount; ++pos)
406 m_links[i].m_jointPos[pos] = q[pos];
407
408 m_links[i].updateCacheMultiDof();
409 }
410
setJointVel(int i,btScalar qdot)411 void btMultiBody::setJointVel(int i, btScalar qdot)
412 {
413 m_realBuf[6 + i] = qdot;
414 }
415
setJointVelMultiDof(int i,btScalar * qdot)416 void btMultiBody::setJointVelMultiDof(int i, btScalar *qdot)
417 {
418 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
419 m_realBuf[6 + m_links[i].m_dofOffset + dof] = qdot[dof];
420 }
421
getRVector(int i) const422 const btVector3 & btMultiBody::getRVector(int i) const
423 {
424 return m_links[i].m_cachedRVector;
425 }
426
getParentToLocalRot(int i) const427 const btQuaternion & btMultiBody::getParentToLocalRot(int i) const
428 {
429 return m_links[i].m_cachedRotParentToThis;
430 }
431
localPosToWorld(int i,const btVector3 & local_pos) const432 btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
433 {
434 btVector3 result = local_pos;
435 while (i != -1) {
436 // 'result' is in frame i. transform it to frame parent(i)
437 result += getRVector(i);
438 result = quatRotate(getParentToLocalRot(i).inverse(),result);
439 i = getParent(i);
440 }
441
442 // 'result' is now in the base frame. transform it to world frame
443 result = quatRotate(getWorldToBaseRot().inverse() ,result);
444 result += getBasePos();
445
446 return result;
447 }
448
worldPosToLocal(int i,const btVector3 & world_pos) const449 btVector3 btMultiBody::worldPosToLocal(int i, const btVector3 &world_pos) const
450 {
451 if (i == -1) {
452 // world to base
453 return quatRotate(getWorldToBaseRot(),(world_pos - getBasePos()));
454 } else {
455 // find position in parent frame, then transform to current frame
456 return quatRotate(getParentToLocalRot(i),worldPosToLocal(getParent(i), world_pos)) - getRVector(i);
457 }
458 }
459
localDirToWorld(int i,const btVector3 & local_dir) const460 btVector3 btMultiBody::localDirToWorld(int i, const btVector3 &local_dir) const
461 {
462 btVector3 result = local_dir;
463 while (i != -1) {
464 result = quatRotate(getParentToLocalRot(i).inverse() , result);
465 i = getParent(i);
466 }
467 result = quatRotate(getWorldToBaseRot().inverse() , result);
468 return result;
469 }
470
worldDirToLocal(int i,const btVector3 & world_dir) const471 btVector3 btMultiBody::worldDirToLocal(int i, const btVector3 &world_dir) const
472 {
473 if (i == -1) {
474 return quatRotate(getWorldToBaseRot(), world_dir);
475 } else {
476 return quatRotate(getParentToLocalRot(i) ,worldDirToLocal(getParent(i), world_dir));
477 }
478 }
479
compTreeLinkVelocities(btVector3 * omega,btVector3 * vel) const480 void btMultiBody::compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
481 {
482 int num_links = getNumLinks();
483 // Calculates the velocities of each link (and the base) in its local frame
484 omega[0] = quatRotate(m_baseQuat ,getBaseOmega());
485 vel[0] = quatRotate(m_baseQuat ,getBaseVel());
486
487 for (int i = 0; i < num_links; ++i) {
488 const int parent = m_links[i].m_parent;
489
490 // transform parent vel into this frame, store in omega[i+1], vel[i+1]
491 SpatialTransform(btMatrix3x3(m_links[i].m_cachedRotParentToThis), m_links[i].m_cachedRVector,
492 omega[parent+1], vel[parent+1],
493 omega[i+1], vel[i+1]);
494
495 // now add qidot * shat_i
496 omega[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
497 vel[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
498 }
499 }
500
getKineticEnergy() const501 btScalar btMultiBody::getKineticEnergy() const
502 {
503 int num_links = getNumLinks();
504 // TODO: would be better not to allocate memory here
505 btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
506 btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
507 compTreeLinkVelocities(&omega[0], &vel[0]);
508
509 // we will do the factor of 0.5 at the end
510 btScalar result = m_baseMass * vel[0].dot(vel[0]);
511 result += omega[0].dot(m_baseInertia * omega[0]);
512
513 for (int i = 0; i < num_links; ++i) {
514 result += m_links[i].m_mass * vel[i+1].dot(vel[i+1]);
515 result += omega[i+1].dot(m_links[i].m_inertiaLocal * omega[i+1]);
516 }
517
518 return 0.5f * result;
519 }
520
getAngularMomentum() const521 btVector3 btMultiBody::getAngularMomentum() const
522 {
523 int num_links = getNumLinks();
524 // TODO: would be better not to allocate memory here
525 btAlignedObjectArray<btVector3> omega;omega.resize(num_links+1);
526 btAlignedObjectArray<btVector3> vel;vel.resize(num_links+1);
527 btAlignedObjectArray<btQuaternion> rot_from_world;rot_from_world.resize(num_links+1);
528 compTreeLinkVelocities(&omega[0], &vel[0]);
529
530 rot_from_world[0] = m_baseQuat;
531 btVector3 result = quatRotate(rot_from_world[0].inverse() , (m_baseInertia * omega[0]));
532
533 for (int i = 0; i < num_links; ++i) {
534 rot_from_world[i+1] = m_links[i].m_cachedRotParentToThis * rot_from_world[m_links[i].m_parent+1];
535 result += (quatRotate(rot_from_world[i+1].inverse() , (m_links[i].m_inertiaLocal * omega[i+1])));
536 }
537
538 return result;
539 }
540
541
clearForcesAndTorques()542 void btMultiBody::clearForcesAndTorques()
543 {
544 m_baseForce.setValue(0, 0, 0);
545 m_baseTorque.setValue(0, 0, 0);
546
547
548 for (int i = 0; i < getNumLinks(); ++i) {
549 m_links[i].m_appliedForce.setValue(0, 0, 0);
550 m_links[i].m_appliedTorque.setValue(0, 0, 0);
551 m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = m_links[i].m_jointTorque[3] = m_links[i].m_jointTorque[4] = m_links[i].m_jointTorque[5] = 0.f;
552 }
553 }
554
clearVelocities()555 void btMultiBody::clearVelocities()
556 {
557 for (int i = 0; i < 6 + getNumLinks(); ++i)
558 {
559 m_realBuf[i] = 0.f;
560 }
561 }
addLinkForce(int i,const btVector3 & f)562 void btMultiBody::addLinkForce(int i, const btVector3 &f)
563 {
564 m_links[i].m_appliedForce += f;
565 }
566
addLinkTorque(int i,const btVector3 & t)567 void btMultiBody::addLinkTorque(int i, const btVector3 &t)
568 {
569 m_links[i].m_appliedTorque += t;
570 }
571
addJointTorque(int i,btScalar Q)572 void btMultiBody::addJointTorque(int i, btScalar Q)
573 {
574 m_links[i].m_jointTorque[0] += Q;
575 }
576
addJointTorqueMultiDof(int i,int dof,btScalar Q)577 void btMultiBody::addJointTorqueMultiDof(int i, int dof, btScalar Q)
578 {
579 m_links[i].m_jointTorque[dof] += Q;
580 }
581
addJointTorqueMultiDof(int i,const btScalar * Q)582 void btMultiBody::addJointTorqueMultiDof(int i, const btScalar *Q)
583 {
584 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
585 m_links[i].m_jointTorque[dof] = Q[dof];
586 }
587
getLinkForce(int i) const588 const btVector3 & btMultiBody::getLinkForce(int i) const
589 {
590 return m_links[i].m_appliedForce;
591 }
592
getLinkTorque(int i) const593 const btVector3 & btMultiBody::getLinkTorque(int i) const
594 {
595 return m_links[i].m_appliedTorque;
596 }
597
getJointTorque(int i) const598 btScalar btMultiBody::getJointTorque(int i) const
599 {
600 return m_links[i].m_jointTorque[0];
601 }
602
getJointTorqueMultiDof(int i)603 btScalar * btMultiBody::getJointTorqueMultiDof(int i)
604 {
605 return &m_links[i].m_jointTorque[0];
606 }
607
outerProduct(const btVector3 & v0,const btVector3 & v1)608 inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //renamed it from vecMulVecTranspose (http://en.wikipedia.org/wiki/Outer_product); maybe it should be moved to btVector3 like dot and cross?
609 {
610 btVector3 row0 = btVector3(
611 v0.x() * v1.x(),
612 v0.x() * v1.y(),
613 v0.x() * v1.z());
614 btVector3 row1 = btVector3(
615 v0.y() * v1.x(),
616 v0.y() * v1.y(),
617 v0.y() * v1.z());
618 btVector3 row2 = btVector3(
619 v0.z() * v1.x(),
620 v0.z() * v1.y(),
621 v0.z() * v1.z());
622
623 btMatrix3x3 m(row0[0],row0[1],row0[2],
624 row1[0],row1[1],row1[2],
625 row2[0],row2[1],row2[2]);
626 return m;
627 }
628
629 #define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
630 //
631
632 #ifndef TEST_SPATIAL_ALGEBRA_LAYER
stepVelocitiesMultiDof(btScalar dt,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m)633 void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
634 btAlignedObjectArray<btScalar> &scratch_r,
635 btAlignedObjectArray<btVector3> &scratch_v,
636 btAlignedObjectArray<btMatrix3x3> &scratch_m)
637 {
638 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
639 // and the base linear & angular accelerations.
640
641 // We apply damping forces in this routine as well as any external forces specified by the
642 // caller (via addBaseForce etc).
643
644 // output should point to an array of 6 + num_links reals.
645 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
646 // num_links joint acceleration values.
647
648 int num_links = getNumLinks();
649
650 const btScalar DAMPING_K1_LINEAR = m_linearDamping;
651 const btScalar DAMPING_K2_LINEAR = m_linearDamping;
652
653 const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
654 const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
655
656 btVector3 base_vel = getBaseVel();
657 btVector3 base_omega = getBaseOmega();
658
659 // Temporary matrices/vectors -- use scratch space from caller
660 // so that we don't have to keep reallocating every frame
661
662 scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
663 scratch_v.resize(8*num_links + 6);
664 scratch_m.resize(4*num_links + 4);
665
666 btScalar * r_ptr = &scratch_r[0];
667 btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
668 btVector3 * v_ptr = &scratch_v[0];
669
670 // vhat_i (top = angular, bottom = linear part)
671 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
672 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
673
674 // zhat_i^A
675 btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
676 btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
677
678 // chat_i (note NOT defined for the base)
679 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
680 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
681
682 // top left, top right and bottom left blocks of Ihat_i^A.
683 // bottom right block = transpose of top left block and is not stored.
684 // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
685 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
686 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
687 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
688
689 // Cached 3x3 rotation matrices from parent frame to this frame.
690 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
691 btMatrix3x3 * rot_from_world = &scratch_m[0];
692
693 // hhat_i, ahat_i
694 // hhat is NOT stored for the base (but ahat is)
695 btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
696 btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
697 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
698 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
699
700 // Y_i, invD_i
701 btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
702 btScalar * Y = &scratch_r[0];
703 /////////////////
704
705 // ptr to the joint accel part of the output
706 btScalar * joint_accel = output + 6;
707
708 // Start of the algorithm proper.
709
710 // First 'upward' loop.
711 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
712
713 rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
714
715 vel_top_angular[0] = rot_from_parent[0] * base_omega;
716 vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
717
718 if (m_fixedBase)
719 {
720 zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
721 }
722 else
723 {
724 zeroAccForce[0] = - (rot_from_parent[0] * (m_baseForce
725 - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
726
727 zeroAccTorque[0] =
728 - (rot_from_parent[0] * m_baseTorque);
729
730 if (m_useGyroTerm)
731 zeroAccTorque[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
732
733 zeroAccTorque[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
734
735 //NOTE: Coriolis terms are missing! (left like so following Stephen's code)
736 }
737
738 inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);
739
740
741 inertia_top_right[0].setValue(m_baseMass, 0, 0,
742 0, m_baseMass, 0,
743 0, 0, m_baseMass);
744 inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
745 0, m_baseInertia[1], 0,
746 0, 0, m_baseInertia[2]);
747
748 rot_from_world[0] = rot_from_parent[0];
749
750 for (int i = 0; i < num_links; ++i) {
751 const int parent = m_links[i].m_parent;
752 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
753
754
755 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
756
757 // vhat_i = i_xhat_p(i) * vhat_p(i)
758 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
759 vel_top_angular[parent+1], vel_bottom_linear[parent+1],
760 vel_top_angular[i+1], vel_bottom_linear[i+1]);
761 //////////////////////////////////////////////////////////////
762
763 // now set vhat_i to its true value by doing
764 // vhat_i += qidot * shat_i
765 btVector3 joint_vel_spat_top, joint_vel_spat_bottom;
766 joint_vel_spat_top.setZero(); joint_vel_spat_bottom.setZero();
767 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
768 {
769 joint_vel_spat_top += getJointVelMultiDof(i)[dof] * m_links[i].getAxisTop(dof);
770 joint_vel_spat_bottom += getJointVelMultiDof(i)[dof] * m_links[i].getAxisBottom(dof);
771 }
772
773 vel_top_angular[i+1] += joint_vel_spat_top;
774 vel_bottom_linear[i+1] += joint_vel_spat_bottom;
775
776 // we can now calculate chat_i
777 // remember vhat_i is really vhat_p(i) (but in current frame) at this point
778 SpatialCrossProduct(vel_top_angular[i+1], vel_bottom_linear[i+1], joint_vel_spat_top, joint_vel_spat_bottom, coriolis_top_angular[i], coriolis_bottom_linear[i]);
779
780 // calculate zhat_i^A
781 //
782 //external forces
783 zeroAccForce[i+1] = -(rot_from_world[i+1] * (m_links[i].m_appliedForce));
784 zeroAccTorque[i+1] = -(rot_from_world[i+1] * m_links[i].m_appliedTorque);
785 //
786 //DAMPING TERMS (ONLY)
787 zeroAccForce[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
788 zeroAccTorque[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
789
790 // calculate Ihat_i^A
791 inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
792 inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
793 0, m_links[i].m_mass, 0,
794 0, 0, m_links[i].m_mass);
795 inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
796 0, m_links[i].m_inertia[1], 0,
797 0, 0, m_links[i].m_inertia[2]);
798
799
800 ////
801 //p += v x* Iv - done in a simpler way
802 if(m_useGyroTerm)
803 zeroAccTorque[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
804 //
805 coriolis_bottom_linear[i] += vel_top_angular[i+1].cross(vel_bottom_linear[i+1]) - (rot_from_parent[i+1] * (vel_top_angular[parent+1].cross(vel_bottom_linear[parent+1])));
806 //coriolis_bottom_linear[i].setZero();
807
808 //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
809 //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
810 //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
811 }
812
813 static btScalar D[36]; //it's dofxdof for each body so asingle 6x6 D matrix will do
814 // 'Downward' loop.
815 // (part of TreeForwardDynamics in Mirtich.)
816 for (int i = num_links - 1; i >= 0; --i)
817 {
818 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
819 {
820 btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
821 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
822
823 //pFunMultSpatVecTimesSpatMat2(m_links[i].m_axesTop[dof], m_links[i].m_axesBottom[dof], inertia_top_left[i+1], inertia_top_right[i+1], inertia_bottom_left[i+1], h_t, h_b);
824 {
825 h_t = inertia_top_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_right[i+1] * m_links[i].getAxisBottom(dof);
826 h_b = inertia_bottom_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(dof);
827 }
828
829 btScalar *D_row = &D[dof * m_links[i].m_dofCount];
830 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
831 {
832 D_row[dof2] = SpatialDotProduct(m_links[i].getAxisTop(dof2), m_links[i].getAxisBottom(dof2), h_t, h_b);
833 }
834
835 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
836 - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
837 - SpatialDotProduct(h_t, h_b, coriolis_top_angular[i], coriolis_bottom_linear[i])
838 ;
839 }
840
841 const int parent = m_links[i].m_parent;
842
843 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
844 switch(m_links[i].m_jointType)
845 {
846 case btMultibodyLink::ePrismatic:
847 case btMultibodyLink::eRevolute:
848 {
849 invDi[0] = 1.0f / D[0];
850 break;
851 }
852 case btMultibodyLink::eSpherical:
853 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
854 case btMultibodyLink::ePlanar:
855 #endif
856 {
857 static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
858 static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
859
860 //unroll the loop?
861 for(int row = 0; row < 3; ++row)
862 for(int col = 0; col < 3; ++col)
863 invDi[row * 3 + col] = invD3x3[row][col];
864
865 break;
866 }
867 default:
868 {
869 }
870 }
871
872
873 static btVector3 tmp_top[6]; //move to scratch mem or other buffers? (12 btVector3 will suffice)
874 static btVector3 tmp_bottom[6];
875
876 //for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
877 //{
878 // tmp_top[dof].setZero();
879 // tmp_bottom[dof].setZero();
880
881 // for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
882 // {
883 // btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
884 // btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
885 // //
886 // tmp_top[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_b;
887 // tmp_bottom[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_t;
888 // }
889 //}
890
891 //btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
892
893 //for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
894 //{
895 // btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
896 // btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
897
898 // TL -= outerProduct(h_t, tmp_top[dof]);
899 // TR -= outerProduct(h_t, tmp_bottom[dof]);
900 // BL -= outerProduct(h_b, tmp_top[dof]);
901 //}
902
903 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
904 {
905 tmp_top[dof].setZero();
906 tmp_bottom[dof].setZero();
907
908 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
909 {
910 btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
911 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
912 //
913 tmp_top[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_t;
914 tmp_bottom[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_b;
915 }
916 }
917
918 btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
919
920 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
921 {
922 btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
923 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
924
925 TL -= outerProduct(h_t, tmp_bottom[dof]);
926 TR -= outerProduct(h_t, tmp_top[dof]);
927 BL -= outerProduct(h_b, tmp_bottom[dof]);
928 }
929
930
931 btMatrix3x3 r_cross;
932 r_cross.setValue(
933 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
934 m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
935 -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
936
937 inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
938 inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
939 inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
940 (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
941
942
943 btVector3 in_top, in_bottom, out_top, out_bottom;
944
945 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
946 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
947 {
948 invD_times_Y[dof] = 0.f;
949
950 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
951 {
952 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
953 }
954 }
955
956 in_top = zeroAccForce[i+1]
957 + inertia_top_left[i+1] * coriolis_top_angular[i]
958 + inertia_top_right[i+1] * coriolis_bottom_linear[i];
959
960 in_bottom = zeroAccTorque[i+1]
961 + inertia_bottom_left[i+1] * coriolis_top_angular[i]
962 + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i];
963
964 //unroll the loop?
965 for(int row = 0; row < 3; ++row)
966 {
967 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
968 {
969 btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
970 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
971
972 in_top[row] += h_t[row] * invD_times_Y[dof];
973 in_bottom[row] += h_b[row] * invD_times_Y[dof];
974 }
975 }
976
977 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
978 in_top, in_bottom, out_top, out_bottom);
979
980 zeroAccForce[parent+1] += out_top;
981 zeroAccTorque[parent+1] += out_bottom;
982 }
983
984
985 // Second 'upward' loop
986 // (part of TreeForwardDynamics in Mirtich)
987
988 if (m_fixedBase)
989 {
990 accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
991 }
992 else
993 {
994 if (num_links > 0)
995 {
996 m_cachedInertiaTopLeft = inertia_top_left[0];
997 m_cachedInertiaTopRight = inertia_top_right[0];
998 m_cachedInertiaLowerLeft = inertia_bottom_left[0];
999 m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
1000
1001 }
1002 btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
1003 btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
1004 float result[6];
1005
1006 solveImatrix(rhs_top, rhs_bot, result);
1007 for (int i = 0; i < 3; ++i) {
1008 accel_top[0][i] = -result[i];
1009 accel_bottom[0][i] = -result[i+3];
1010 }
1011
1012 }
1013
1014 static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough
1015 // now do the loop over the m_links
1016 for (int i = 0; i < num_links; ++i) {
1017 const int parent = m_links[i].m_parent;
1018
1019 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1020 accel_top[parent+1], accel_bottom[parent+1],
1021 accel_top[i+1], accel_bottom[i+1]);
1022
1023 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1024 {
1025 btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
1026 btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
1027
1028 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
1029 }
1030
1031 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1032 mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1033
1034 accel_top[i+1] += coriolis_top_angular[i];
1035 accel_bottom[i+1] += coriolis_bottom_linear[i];
1036
1037 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1038 {
1039 accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
1040 accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
1041 }
1042 }
1043
1044 // transform base accelerations back to the world frame.
1045 btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
1046 output[0] = omegadot_out[0];
1047 output[1] = omegadot_out[1];
1048 output[2] = omegadot_out[2];
1049
1050 btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
1051 output[3] = vdot_out[0];
1052 output[4] = vdot_out[1];
1053 output[5] = vdot_out[2];
1054
1055 /////////////////
1056 //printf("q = [");
1057 //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1058 //for(int link = 0; link < getNumLinks(); ++link)
1059 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1060 // printf("%.6f ", m_links[link].m_jointPos[dof]);
1061 //printf("]\n");
1062 ////
1063 //printf("qd = [");
1064 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1065 // printf("%.6f ", m_realBuf[dof]);
1066 //printf("]\n");
1067 //printf("qdd = [");
1068 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1069 // printf("%.6f ", output[dof]);
1070 //printf("]\n");
1071 /////////////////
1072
1073 // Final step: add the accelerations (times dt) to the velocities.
1074 applyDeltaVeeMultiDof(output, dt);
1075 }
1076
1077 #else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
stepVelocitiesMultiDof(btScalar dt,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m)1078 void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
1079 btAlignedObjectArray<btScalar> &scratch_r,
1080 btAlignedObjectArray<btVector3> &scratch_v,
1081 btAlignedObjectArray<btMatrix3x3> &scratch_m)
1082 {
1083 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
1084 // and the base linear & angular accelerations.
1085
1086 // We apply damping forces in this routine as well as any external forces specified by the
1087 // caller (via addBaseForce etc).
1088
1089 // output should point to an array of 6 + num_links reals.
1090 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
1091 // num_links joint acceleration values.
1092
1093 int num_links = getNumLinks();
1094
1095 const btScalar DAMPING_K1_LINEAR = m_linearDamping;
1096 const btScalar DAMPING_K2_LINEAR = m_linearDamping;
1097
1098 const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
1099 const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
1100
1101 btVector3 base_vel = getBaseVel();
1102 btVector3 base_omega = getBaseOmega();
1103
1104 // Temporary matrices/vectors -- use scratch space from caller
1105 // so that we don't have to keep reallocating every frame
1106
1107 scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
1108 scratch_v.resize(8*num_links + 6);
1109 scratch_m.resize(4*num_links + 4);
1110
1111 //btScalar * r_ptr = &scratch_r[0];
1112 btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
1113 btVector3 * v_ptr = &scratch_v[0];
1114
1115 // vhat_i (top = angular, bottom = linear part)
1116 btSpatialMotionVector *spatVel = (btSpatialMotionVector *)v_ptr;
1117 v_ptr += num_links * 2 + 2;
1118 //
1119 // zhat_i^A
1120 btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
1121 v_ptr += num_links * 2 + 2;
1122 //
1123 // chat_i (note NOT defined for the base)
1124 btSpatialMotionVector * spatCoriolisAcc = (btSpatialMotionVector *)v_ptr;
1125 v_ptr += num_links * 2;
1126 //
1127 // Ihat_i^A.
1128 btSymmetricSpatialDyad * spatInertia = (btSymmetricSpatialDyad *)&scratch_m[num_links + 1];
1129
1130 // Cached 3x3 rotation matrices from parent frame to this frame.
1131 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1132 btMatrix3x3 * rot_from_world = &scratch_m[0];
1133
1134 // hhat_i, ahat_i
1135 // hhat is NOT stored for the base (but ahat is)
1136 btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
1137 btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
1138 v_ptr += num_links * 2 + 2;
1139 //
1140 // Y_i, invD_i
1141 btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1142 btScalar * Y = &scratch_r[0];
1143 //
1144 //aux variables
1145 static btSpatialMotionVector spatJointVel; //spatial velocity due to the joint motion (i.e. without predecessors' influence)
1146 static btScalar D[36]; //"D" matrix; it's dofxdof for each body so asingle 6x6 D matrix will do
1147 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
1148 static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
1149 static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
1150 static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
1151 static btSpatialTransformationMatrix fromParent; //spatial transform from parent to child
1152 static btSymmetricSpatialDyad dyadTemp; //inertia matrix temp
1153 static btSpatialTransformationMatrix fromWorld;
1154 fromWorld.m_trnVec.setZero();
1155 /////////////////
1156
1157 // ptr to the joint accel part of the output
1158 btScalar * joint_accel = output + 6;
1159
1160 // Start of the algorithm proper.
1161
1162 // First 'upward' loop.
1163 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1164
1165 rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
1166
1167 //create the vector of spatial velocity of the base by transforming global-coor linear and angular velocities into base-local coordinates
1168 spatVel[0].setVector(rot_from_parent[0] * base_omega, rot_from_parent[0] * base_vel);
1169
1170 if (m_fixedBase)
1171 {
1172 zeroAccSpatFrc[0].setZero();
1173 }
1174 else
1175 {
1176 //external forces
1177 zeroAccSpatFrc[0].setVector(-(rot_from_parent[0] * m_baseTorque), -(rot_from_parent[0] * m_baseForce));
1178
1179 //adding damping terms (only)
1180 btScalar linDampMult = 1., angDampMult = 1.;
1181 zeroAccSpatFrc[0].addVector(angDampMult * m_baseInertia * spatVel[0].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[0].getAngular().norm()),
1182 linDampMult * m_baseMass * spatVel[0].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[0].getLinear().norm()));
1183
1184 //
1185 //p += vhat x Ihat vhat - done in a simpler way
1186 if (m_useGyroTerm)
1187 zeroAccSpatFrc[0].addAngular(spatVel[0].getAngular().cross(m_baseInertia * spatVel[0].getAngular()));
1188 //
1189 zeroAccSpatFrc[0].addLinear(m_baseMass * spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1190 }
1191
1192
1193 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
1194 spatInertia[0].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
1195 //
1196 btMatrix3x3(m_baseMass, 0, 0,
1197 0, m_baseMass, 0,
1198 0, 0, m_baseMass),
1199 //
1200 btMatrix3x3(m_baseInertia[0], 0, 0,
1201 0, m_baseInertia[1], 0,
1202 0, 0, m_baseInertia[2])
1203 );
1204
1205 rot_from_world[0] = rot_from_parent[0];
1206
1207 //
1208 for (int i = 0; i < num_links; ++i) {
1209 const int parent = m_links[i].m_parent;
1210 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
1211 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1212
1213 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1214 fromWorld.m_rotMat = rot_from_world[i+1];
1215 fromParent.transform(spatVel[parent+1], spatVel[i+1]);
1216
1217 // now set vhat_i to its true value by doing
1218 // vhat_i += qidot * shat_i
1219 if(!m_useGlobalVelocities)
1220 {
1221 spatJointVel.setZero();
1222
1223 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1224 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1225
1226 // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
1227 spatVel[i+1] += spatJointVel;
1228
1229 //
1230 // vhat_i is vhat_p(i) transformed to local coors + the velocity across the i-th inboard joint
1231 //spatVel[i+1] = fromParent * spatVel[parent+1] + spatJointVel;
1232
1233 }
1234 else
1235 {
1236 fromWorld.transformRotationOnly(m_links[i].m_absFrameTotVelocity, spatVel[i+1]);
1237 fromWorld.transformRotationOnly(m_links[i].m_absFrameLocVelocity, spatJointVel);
1238 }
1239
1240 // we can now calculate chat_i
1241 spatVel[i+1].cross(spatJointVel, spatCoriolisAcc[i]);
1242
1243 // calculate zhat_i^A
1244 //
1245 //external forces
1246 zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
1247 //
1248 //adding damping terms (only)
1249 btScalar linDampMult = 1., angDampMult = 1.;
1250 zeroAccSpatFrc[i+1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i+1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i+1].getAngular().norm()),
1251 linDampMult * m_links[i].m_mass * spatVel[i+1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i+1].getLinear().norm()));
1252
1253 // calculate Ihat_i^A
1254 //init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
1255 spatInertia[i+1].setMatrix( btMatrix3x3(0,0,0,0,0,0,0,0,0),
1256 //
1257 btMatrix3x3(m_links[i].m_mass, 0, 0,
1258 0, m_links[i].m_mass, 0,
1259 0, 0, m_links[i].m_mass),
1260 //
1261 btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
1262 0, m_links[i].m_inertiaLocal[1], 0,
1263 0, 0, m_links[i].m_inertiaLocal[2])
1264 );
1265 //
1266 //p += vhat x Ihat vhat - done in a simpler way
1267 if(m_useGyroTerm)
1268 zeroAccSpatFrc[i+1].addAngular(spatVel[i+1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i+1].getAngular()));
1269 //
1270 zeroAccSpatFrc[i+1].addLinear(m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear()));
1271 //btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
1272 ////clamp parent's omega
1273 //btScalar parOmegaMod = temp.length();
1274 //btScalar parOmegaModMax = 1000;
1275 //if(parOmegaMod > parOmegaModMax)
1276 // temp *= parOmegaModMax / parOmegaMod;
1277 //zeroAccSpatFrc[i+1].addLinear(temp);
1278 //printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
1279 //temp = spatCoriolisAcc[i].getLinear();
1280 //printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
1281
1282
1283
1284 //printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
1285 //printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
1286 //printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
1287 }
1288
1289 // 'Downward' loop.
1290 // (part of TreeForwardDynamics in Mirtich.)
1291 for (int i = num_links - 1; i >= 0; --i)
1292 {
1293 const int parent = m_links[i].m_parent;
1294 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1295
1296 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1297 {
1298 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1299 //
1300 hDof = spatInertia[i+1] * m_links[i].m_axes[dof];
1301 //
1302 Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
1303 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
1304 - spatCoriolisAcc[i].dot(hDof)
1305 ;
1306 }
1307
1308 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1309 {
1310 btScalar *D_row = &D[dof * m_links[i].m_dofCount];
1311 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1312 {
1313 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
1314 D_row[dof2] = m_links[i].m_axes[dof].dot(hDof2);
1315 }
1316 }
1317
1318 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1319 switch(m_links[i].m_jointType)
1320 {
1321 case btMultibodyLink::ePrismatic:
1322 case btMultibodyLink::eRevolute:
1323 {
1324 invDi[0] = 1.0f / D[0];
1325 break;
1326 }
1327 case btMultibodyLink::eSpherical:
1328 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
1329 case btMultibodyLink::ePlanar:
1330 #endif
1331 {
1332 static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
1333 static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
1334
1335 //unroll the loop?
1336 for(int row = 0; row < 3; ++row)
1337 {
1338 for(int col = 0; col < 3; ++col)
1339 {
1340 invDi[row * 3 + col] = invD3x3[row][col];
1341 }
1342 }
1343
1344 break;
1345 }
1346 default:
1347 {
1348
1349 }
1350 }
1351
1352 //determine h*D^{-1}
1353 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1354 {
1355 spatForceVecTemps[dof].setZero();
1356
1357 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1358 {
1359 btSpatialForceVector &hDof2 = h[m_links[i].m_dofOffset + dof2];
1360 //
1361 spatForceVecTemps[dof] += hDof2 * invDi[dof2 * m_links[i].m_dofCount + dof];
1362 }
1363 }
1364
1365 dyadTemp = spatInertia[i+1];
1366
1367 //determine (h*D^{-1}) * h^{T}
1368 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1369 {
1370 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1371 //
1372 dyadTemp -= symmetricSpatialOuterProduct(hDof, spatForceVecTemps[dof]);
1373 }
1374
1375 fromParent.transformInverse(dyadTemp, spatInertia[parent+1], btSpatialTransformationMatrix::Add);
1376
1377 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1378 {
1379 invD_times_Y[dof] = 0.f;
1380
1381 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
1382 {
1383 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
1384 }
1385 }
1386
1387 spatForceVecTemps[0] = zeroAccSpatFrc[i+1] + spatInertia[i+1] * spatCoriolisAcc[i];
1388
1389 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1390 {
1391 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1392 //
1393 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
1394 }
1395
1396 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
1397
1398 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
1399 }
1400
1401
1402 // Second 'upward' loop
1403 // (part of TreeForwardDynamics in Mirtich)
1404
1405 if (m_fixedBase)
1406 {
1407 spatAcc[0].setZero();
1408 }
1409 else
1410 {
1411 if (num_links > 0)
1412 {
1413 m_cachedInertiaTopLeft = spatInertia[0].m_topLeftMat;
1414 m_cachedInertiaTopRight = spatInertia[0].m_topRightMat;
1415 m_cachedInertiaLowerLeft = spatInertia[0].m_bottomLeftMat;
1416 m_cachedInertiaLowerRight= spatInertia[0].m_topLeftMat.transpose();
1417
1418 }
1419
1420 solveImatrix(zeroAccSpatFrc[0], result);
1421 spatAcc[0] = -result;
1422 }
1423
1424 // now do the loop over the m_links
1425 for (int i = 0; i < num_links; ++i)
1426 {
1427 // qdd = D^{-1} * (Y - h^{T}*apar) = (S^{T}*I*S)^{-1} * (tau - S^{T}*I*cor - S^{T}*zeroAccFrc - S^{T}*I*apar)
1428 // a = apar + cor + Sqdd
1429 //or
1430 // qdd = D^{-1} * (Y - h^{T}*(apar+cor))
1431 // a = apar + Sqdd
1432
1433 const int parent = m_links[i].m_parent;
1434 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1435
1436 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
1437
1438 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1439 {
1440 btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
1441 //
1442 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
1443 }
1444
1445 btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
1446 //D^{-1} * (Y - h^{T}*apar)
1447 mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
1448
1449 spatAcc[i+1] += spatCoriolisAcc[i];
1450
1451 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1452 spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
1453 }
1454
1455 // transform base accelerations back to the world frame.
1456 btVector3 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
1457 output[0] = omegadot_out[0];
1458 output[1] = omegadot_out[1];
1459 output[2] = omegadot_out[2];
1460
1461 btVector3 vdot_out = rot_from_parent[0].transpose() * (spatAcc[0].getLinear() + spatVel[0].getAngular().cross(spatVel[0].getLinear()));
1462 output[3] = vdot_out[0];
1463 output[4] = vdot_out[1];
1464 output[5] = vdot_out[2];
1465
1466 /////////////////
1467 //printf("q = [");
1468 //printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1469 //for(int link = 0; link < getNumLinks(); ++link)
1470 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1471 // printf("%.6f ", m_links[link].m_jointPos[dof]);
1472 //printf("]\n");
1473 ////
1474 //printf("qd = [");
1475 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1476 // printf("%.6f ", m_realBuf[dof]);
1477 //printf("]\n");
1478 //printf("qdd = [");
1479 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
1480 // printf("%.6f ", output[dof]);
1481 //printf("]\n");
1482 /////////////////
1483
1484 // Final step: add the accelerations (times dt) to the velocities.
1485 if(dt > 0.)
1486 applyDeltaVeeMultiDof(output, dt);
1487
1488 /////
1489 //btScalar angularThres = 1;
1490 //btScalar maxAngVel = 0.;
1491 //bool scaleDown = 1.;
1492 //for(int link = 0; link < m_links.size(); ++link)
1493 //{
1494 // if(spatVel[link+1].getAngular().length() > maxAngVel)
1495 // {
1496 // maxAngVel = spatVel[link+1].getAngular().length();
1497 // scaleDown = angularThres / spatVel[link+1].getAngular().length();
1498 // break;
1499 // }
1500 //}
1501
1502 //if(scaleDown != 1.)
1503 //{
1504 // for(int link = 0; link < m_links.size(); ++link)
1505 // {
1506 // if(m_links[link].m_jointType == btMultibodyLink::eRevolute || m_links[link].m_jointType == btMultibodyLink::eSpherical)
1507 // {
1508 // for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
1509 // getJointVelMultiDof(link)[dof] *= scaleDown;
1510 // }
1511 // }
1512 //}
1513 /////
1514
1515 /////////////////////
1516 if(m_useGlobalVelocities)
1517 {
1518 for (int i = 0; i < num_links; ++i)
1519 {
1520 const int parent = m_links[i].m_parent;
1521 //rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis); /// <- done
1522 //rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1]; /// <- done
1523
1524 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
1525 fromWorld.m_rotMat = rot_from_world[i+1];
1526
1527 // vhat_i = i_xhat_p(i) * vhat_p(i)
1528 fromParent.transform(spatVel[parent+1], spatVel[i+1]);
1529 //nice alternative below (using operator *) but it generates temps
1530 /////////////////////////////////////////////////////////////
1531
1532 // now set vhat_i to its true value by doing
1533 // vhat_i += qidot * shat_i
1534 spatJointVel.setZero();
1535
1536 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
1537 spatJointVel += m_links[i].m_axes[dof] * getJointVelMultiDof(i)[dof];
1538
1539 // remember vhat_i is really vhat_p(i) (but in current frame) at this point => we need to add velocity across the inboard joint
1540 spatVel[i+1] += spatJointVel;
1541
1542
1543 fromWorld.transformInverseRotationOnly(spatVel[i+1], m_links[i].m_absFrameTotVelocity);
1544 fromWorld.transformInverseRotationOnly(spatJointVel, m_links[i].m_absFrameLocVelocity);
1545 }
1546 }
1547
1548 }
1549 #endif
1550
1551
stepVelocities(btScalar dt,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m)1552 void btMultiBody::stepVelocities(btScalar dt,
1553 btAlignedObjectArray<btScalar> &scratch_r,
1554 btAlignedObjectArray<btVector3> &scratch_v,
1555 btAlignedObjectArray<btMatrix3x3> &scratch_m)
1556 {
1557 // Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
1558 // and the base linear & angular accelerations.
1559
1560 // We apply damping forces in this routine as well as any external forces specified by the
1561 // caller (via addBaseForce etc).
1562
1563 // output should point to an array of 6 + num_links reals.
1564 // Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
1565 // num_links joint acceleration values.
1566
1567 int num_links = getNumLinks();
1568
1569 const btScalar DAMPING_K1_LINEAR = m_linearDamping;
1570 const btScalar DAMPING_K2_LINEAR = m_linearDamping;
1571
1572 const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
1573 const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
1574
1575 btVector3 base_vel = getBaseVel();
1576 btVector3 base_omega = getBaseOmega();
1577
1578 // Temporary matrices/vectors -- use scratch space from caller
1579 // so that we don't have to keep reallocating every frame
1580
1581 scratch_r.resize(2*num_links + 6);
1582 scratch_v.resize(8*num_links + 6);
1583 scratch_m.resize(4*num_links + 4);
1584
1585 btScalar * r_ptr = &scratch_r[0];
1586 btScalar * output = &scratch_r[num_links]; // "output" holds the q_double_dot results
1587 btVector3 * v_ptr = &scratch_v[0];
1588
1589 // vhat_i (top = angular, bottom = linear part)
1590 btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
1591 btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
1592
1593 // zhat_i^A
1594 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
1595 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
1596
1597 // chat_i (note NOT defined for the base)
1598 btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
1599 btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
1600
1601 // top left, top right and bottom left blocks of Ihat_i^A.
1602 // bottom right block = transpose of top left block and is not stored.
1603 // Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
1604 btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
1605 btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
1606 btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
1607
1608 // Cached 3x3 rotation matrices from parent frame to this frame.
1609 btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1610 btMatrix3x3 * rot_from_world = &scratch_m[0];
1611
1612 // hhat_i, ahat_i
1613 // hhat is NOT stored for the base (but ahat is)
1614 btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
1615 btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
1616 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1617 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1618
1619 // Y_i, D_i
1620 btScalar * Y = r_ptr; r_ptr += num_links;
1621 btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
1622
1623 // ptr to the joint accel part of the output
1624 btScalar * joint_accel = output + 6;
1625
1626
1627 // Start of the algorithm proper.
1628
1629 // First 'upward' loop.
1630 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
1631
1632 rot_from_parent[0] = btMatrix3x3(m_baseQuat);
1633
1634 vel_top_angular[0] = rot_from_parent[0] * base_omega;
1635 vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
1636
1637 if (m_fixedBase) {
1638 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
1639 } else {
1640 zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
1641 - m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
1642
1643 zero_acc_bottom_linear[0] =
1644 - (rot_from_parent[0] * m_baseTorque);
1645
1646 if (m_useGyroTerm)
1647 zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
1648
1649 zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
1650
1651 }
1652
1653
1654
1655 inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
1656
1657
1658 inertia_top_right[0].setValue(m_baseMass, 0, 0,
1659 0, m_baseMass, 0,
1660 0, 0, m_baseMass);
1661 inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
1662 0, m_baseInertia[1], 0,
1663 0, 0, m_baseInertia[2]);
1664
1665 rot_from_world[0] = rot_from_parent[0];
1666
1667 for (int i = 0; i < num_links; ++i) {
1668 const int parent = m_links[i].m_parent;
1669 rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
1670
1671
1672 rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
1673
1674 // vhat_i = i_xhat_p(i) * vhat_p(i)
1675 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1676 vel_top_angular[parent+1], vel_bottom_linear[parent+1],
1677 vel_top_angular[i+1], vel_bottom_linear[i+1]);
1678
1679 // we can now calculate chat_i
1680 // remember vhat_i is really vhat_p(i) (but in current frame) at this point
1681 coriolis_bottom_linear[i] = vel_top_angular[i+1].cross(vel_top_angular[i+1].cross(m_links[i].m_cachedRVector))
1682 + 2 * vel_top_angular[i+1].cross(m_links[i].getAxisBottom(0)) * getJointVel(i);
1683 if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
1684 coriolis_top_angular[i] = vel_top_angular[i+1].cross(m_links[i].getAxisTop(0)) * getJointVel(i);
1685 coriolis_bottom_linear[i] += (getJointVel(i) * getJointVel(i)) * m_links[i].getAxisTop(0).cross(m_links[i].getAxisBottom(0));
1686 } else {
1687 coriolis_top_angular[i] = btVector3(0,0,0);
1688 }
1689
1690 // now set vhat_i to its true value by doing
1691 // vhat_i += qidot * shat_i
1692 vel_top_angular[i+1] += getJointVel(i) * m_links[i].getAxisTop(0);
1693 vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
1694
1695 // calculate zhat_i^A
1696 zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
1697 zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
1698
1699 zero_acc_bottom_linear[i+1] =
1700 - (rot_from_world[i+1] * m_links[i].m_appliedTorque);
1701 if (m_useGyroTerm)
1702 {
1703 zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
1704 }
1705
1706 zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
1707
1708 // calculate Ihat_i^A
1709 inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
1710 inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
1711 0, m_links[i].m_mass, 0,
1712 0, 0, m_links[i].m_mass);
1713 inertia_bottom_left[i+1].setValue(m_links[i].m_inertiaLocal[0], 0, 0,
1714 0, m_links[i].m_inertiaLocal[1], 0,
1715 0, 0, m_links[i].m_inertiaLocal[2]);
1716 }
1717
1718
1719 // 'Downward' loop.
1720 // (part of TreeForwardDynamics in Mirtich.)
1721 for (int i = num_links - 1; i >= 0; --i) {
1722
1723 h_top[i] = inertia_top_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_right[i+1] * m_links[i].getAxisBottom(0);
1724 h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
1725 btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
1726 D[i] = val;
1727
1728 Y[i] = m_links[i].m_jointTorque[0]
1729 - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
1730 - SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
1731
1732 const int parent = m_links[i].m_parent;
1733
1734 btAssert(D[i]!=0.f);
1735 // Ip += pXi * (Ii - hi hi' / Di) * iXp
1736 const btScalar one_over_di = 1.0f / D[i];
1737
1738
1739
1740
1741 const btMatrix3x3 TL = inertia_top_left[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_bottom[i]);
1742 const btMatrix3x3 TR = inertia_top_right[i+1] - vecMulVecTranspose(one_over_di * h_top[i] , h_top[i]);
1743 const btMatrix3x3 BL = inertia_bottom_left[i+1]- vecMulVecTranspose(one_over_di * h_bottom[i] , h_bottom[i]);
1744
1745
1746 btMatrix3x3 r_cross;
1747 r_cross.setValue(
1748 0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
1749 m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
1750 -m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
1751
1752 inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
1753 inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
1754 inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
1755 (r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
1756
1757
1758 // Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
1759 btVector3 in_top, in_bottom, out_top, out_bottom;
1760 const btScalar Y_over_D = Y[i] * one_over_di;
1761 in_top = zero_acc_top_angular[i+1]
1762 + inertia_top_left[i+1] * coriolis_top_angular[i]
1763 + inertia_top_right[i+1] * coriolis_bottom_linear[i]
1764 + Y_over_D * h_top[i];
1765 in_bottom = zero_acc_bottom_linear[i+1]
1766 + inertia_bottom_left[i+1] * coriolis_top_angular[i]
1767 + inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
1768 + Y_over_D * h_bottom[i];
1769 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1770 in_top, in_bottom, out_top, out_bottom);
1771 zero_acc_top_angular[parent+1] += out_top;
1772 zero_acc_bottom_linear[parent+1] += out_bottom;
1773 }
1774
1775
1776 // Second 'upward' loop
1777 // (part of TreeForwardDynamics in Mirtich)
1778
1779 if (m_fixedBase)
1780 {
1781 accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
1782 }
1783 else
1784 {
1785 if (num_links > 0)
1786 {
1787 //Matrix<btScalar, 6, 6> Imatrix;
1788 //Imatrix.block<3,3>(0,0) = inertia_top_left[0];
1789 //Imatrix.block<3,3>(3,0) = inertia_bottom_left[0];
1790 //Imatrix.block<3,3>(0,3) = inertia_top_right[0];
1791 //Imatrix.block<3,3>(3,3) = inertia_top_left[0].transpose();
1792 //cached_imatrix_lu.reset(new Eigen::LU<Matrix<btScalar, 6, 6> >(Imatrix)); // TODO: Avoid memory allocation here?
1793
1794 m_cachedInertiaTopLeft = inertia_top_left[0];
1795 m_cachedInertiaTopRight = inertia_top_right[0];
1796 m_cachedInertiaLowerLeft = inertia_bottom_left[0];
1797 m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
1798
1799 }
1800 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
1801 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
1802 float result[6];
1803
1804 solveImatrix(rhs_top, rhs_bot, result);
1805 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
1806 for (int i = 0; i < 3; ++i) {
1807 accel_top[0][i] = -result[i];
1808 accel_bottom[0][i] = -result[i+3];
1809 }
1810
1811 }
1812
1813 // now do the loop over the m_links
1814 for (int i = 0; i < num_links; ++i) {
1815 const int parent = m_links[i].m_parent;
1816 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
1817 accel_top[parent+1], accel_bottom[parent+1],
1818 accel_top[i+1], accel_bottom[i+1]);
1819 joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
1820 accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
1821 accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
1822 }
1823
1824 // transform base accelerations back to the world frame.
1825 btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
1826 output[0] = omegadot_out[0];
1827 output[1] = omegadot_out[1];
1828 output[2] = omegadot_out[2];
1829
1830 btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
1831 output[3] = vdot_out[0];
1832 output[4] = vdot_out[1];
1833 output[5] = vdot_out[2];
1834
1835 /////////////////
1836 //printf("q = [");
1837 //printf("%.2f, %.2f, %.2f, %.2f, %.2f, %.2f, %.2f", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
1838 //for(int link = 0; link < getNumLinks(); ++link)
1839 // printf("%.2f ", m_links[link].m_jointPos[0]);
1840 //printf("]\n");
1841 ////
1842 //printf("qd = [");
1843 //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1844 // printf("%.2f ", m_realBuf[dof]);
1845 //printf("]\n");
1846 ////
1847 //printf("qdd = [");
1848 //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1849 // printf("%.2f ", output[dof]);
1850 //printf("]\n");
1851 /////////////////
1852
1853 // Final step: add the accelerations (times dt) to the velocities.
1854 applyDeltaVee(output, dt);
1855
1856
1857 }
1858
solveImatrix(const btVector3 & rhs_top,const btVector3 & rhs_bot,float result[6]) const1859 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
1860 {
1861 int num_links = getNumLinks();
1862 ///solve I * x = rhs, so the result = invI * rhs
1863 if (num_links == 0)
1864 {
1865 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1866 result[0] = rhs_bot[0] / m_baseInertia[0];
1867 result[1] = rhs_bot[1] / m_baseInertia[1];
1868 result[2] = rhs_bot[2] / m_baseInertia[2];
1869 result[3] = rhs_top[0] / m_baseMass;
1870 result[4] = rhs_top[1] / m_baseMass;
1871 result[5] = rhs_top[2] / m_baseMass;
1872 } else
1873 {
1874 /// Special routine for calculating the inverse of a spatial inertia matrix
1875 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
1876 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
1877 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1878 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
1879 tmp = invIupper_right * m_cachedInertiaLowerRight;
1880 btMatrix3x3 invI_upper_left = (tmp * Binv);
1881 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1882 tmp = m_cachedInertiaTopLeft * invI_upper_left;
1883 tmp[0][0]-= 1.0;
1884 tmp[1][1]-= 1.0;
1885 tmp[2][2]-= 1.0;
1886 btMatrix3x3 invI_lower_left = (Binv * tmp);
1887
1888 //multiply result = invI * rhs
1889 {
1890 btVector3 vtop = invI_upper_left*rhs_top;
1891 btVector3 tmp;
1892 tmp = invIupper_right * rhs_bot;
1893 vtop += tmp;
1894 btVector3 vbot = invI_lower_left*rhs_top;
1895 tmp = invI_lower_right * rhs_bot;
1896 vbot += tmp;
1897 result[0] = vtop[0];
1898 result[1] = vtop[1];
1899 result[2] = vtop[2];
1900 result[3] = vbot[0];
1901 result[4] = vbot[1];
1902 result[5] = vbot[2];
1903 }
1904
1905 }
1906 }
1907 #ifdef TEST_SPATIAL_ALGEBRA_LAYER
solveImatrix(const btSpatialForceVector & rhs,btSpatialMotionVector & result) const1908 void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
1909 {
1910 int num_links = getNumLinks();
1911 ///solve I * x = rhs, so the result = invI * rhs
1912 if (num_links == 0)
1913 {
1914 // in the case of 0 m_links (i.e. a plain rigid body, not a multibody) rhs * invI is easier
1915 result.setAngular(rhs.getAngular() / m_baseInertia);
1916 result.setLinear(rhs.getLinear() / m_baseMass);
1917 } else
1918 {
1919 /// Special routine for calculating the inverse of a spatial inertia matrix
1920 ///the 6x6 matrix is stored as 4 blocks of 3x3 matrices
1921 btMatrix3x3 Binv = m_cachedInertiaTopRight.inverse()*-1.f;
1922 btMatrix3x3 tmp = m_cachedInertiaLowerRight * Binv;
1923 btMatrix3x3 invIupper_right = (tmp * m_cachedInertiaTopLeft + m_cachedInertiaLowerLeft).inverse();
1924 tmp = invIupper_right * m_cachedInertiaLowerRight;
1925 btMatrix3x3 invI_upper_left = (tmp * Binv);
1926 btMatrix3x3 invI_lower_right = (invI_upper_left).transpose();
1927 tmp = m_cachedInertiaTopLeft * invI_upper_left;
1928 tmp[0][0]-= 1.0;
1929 tmp[1][1]-= 1.0;
1930 tmp[2][2]-= 1.0;
1931 btMatrix3x3 invI_lower_left = (Binv * tmp);
1932
1933 //multiply result = invI * rhs
1934 {
1935 btVector3 vtop = invI_upper_left*rhs.getLinear();
1936 btVector3 tmp;
1937 tmp = invIupper_right * rhs.getAngular();
1938 vtop += tmp;
1939 btVector3 vbot = invI_lower_left*rhs.getLinear();
1940 tmp = invI_lower_right * rhs.getAngular();
1941 vbot += tmp;
1942 result.setVector(vtop, vbot);
1943 }
1944
1945 }
1946 }
1947 #endif
1948
mulMatrix(btScalar * pA,btScalar * pB,int rowsA,int colsA,int rowsB,int colsB,btScalar * pC) const1949 void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
1950 {
1951 for (int row = 0; row < rowsA; row++)
1952 {
1953 for (int col = 0; col < colsB; col++)
1954 {
1955 pC[row * colsB + col] = 0.f;
1956 for (int inner = 0; inner < rowsB; inner++)
1957 {
1958 pC[row * colsB + col] += pA[row * colsA + inner] * pB[col + inner * colsB];
1959 }
1960 }
1961 }
1962 }
1963
1964 #ifndef TEST_SPATIAL_ALGEBRA_LAYER
calcAccelerationDeltasMultiDof(const btScalar * force,btScalar * output,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v) const1965 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
1966 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
1967 {
1968 // Temporary matrices/vectors -- use scratch space from caller
1969 // so that we don't have to keep reallocating every frame
1970
1971 int num_links = getNumLinks();
1972 int m_dofCount = getNumDofs();
1973 scratch_r.resize(m_dofCount);
1974 scratch_v.resize(4*num_links + 4);
1975
1976 btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
1977 btVector3 * v_ptr = &scratch_v[0];
1978
1979 // zhat_i^A (scratch space)
1980 btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
1981 btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
1982
1983 // rot_from_parent (cached from calcAccelerations)
1984 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
1985
1986 // hhat (cached), accel (scratch)
1987 // hhat is NOT stored for the base (but ahat is)
1988 const btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
1989 const btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
1990 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
1991 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
1992
1993 // Y_i (scratch), invD_i (cached)
1994 const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
1995 btScalar * Y = r_ptr;
1996 ////////////////
1997
1998 // First 'upward' loop.
1999 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
2000
2001 btVector3 input_force(force[3],force[4],force[5]);
2002 btVector3 input_torque(force[0],force[1],force[2]);
2003
2004 // Fill in zero_acc
2005 // -- set to force/torque on the base, zero otherwise
2006 if (m_fixedBase)
2007 {
2008 zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
2009 } else
2010 {
2011 zeroAccForce[0] = - (rot_from_parent[0] * input_force);
2012 zeroAccTorque[0] = - (rot_from_parent[0] * input_torque);
2013 }
2014 for (int i = 0; i < num_links; ++i)
2015 {
2016 zeroAccForce[i+1] = zeroAccTorque[i+1] = btVector3(0,0,0);
2017 }
2018
2019 // 'Downward' loop.
2020 // (part of TreeForwardDynamics in Mirtich.)
2021 for (int i = num_links - 1; i >= 0; --i)
2022 {
2023 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2024 {
2025 //?? btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
2026
2027 Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
2028 - SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
2029 ;
2030 }
2031
2032 btScalar aa = Y[i];
2033 const int parent = m_links[i].m_parent;
2034
2035 btVector3 in_top, in_bottom, out_top, out_bottom;
2036 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
2037
2038 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
2039 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2040 {
2041 invD_times_Y[dof] = 0.f;
2042
2043 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
2044 {
2045 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
2046 }
2047 }
2048
2049 // Zp += pXi * (Zi + hi*Yi/Di)
2050 in_top = zeroAccForce[i+1];
2051 in_bottom = zeroAccTorque[i+1];
2052
2053 //unroll the loop?
2054 for(int row = 0; row < 3; ++row)
2055 {
2056 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2057 {
2058 const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
2059 const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
2060
2061 in_top[row] += h_t[row] * invD_times_Y[dof];
2062 in_bottom[row] += h_b[row] * invD_times_Y[dof];
2063 }
2064 }
2065
2066 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
2067 in_top, in_bottom, out_top, out_bottom);
2068 zeroAccForce[parent+1] += out_top;
2069 zeroAccTorque[parent+1] += out_bottom;
2070 }
2071
2072 // ptr to the joint accel part of the output
2073 btScalar * joint_accel = output + 6;
2074
2075
2076 // Second 'upward' loop
2077 // (part of TreeForwardDynamics in Mirtich)
2078
2079 if (m_fixedBase)
2080 {
2081 accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
2082 }
2083 else
2084 {
2085 btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
2086 btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
2087 float result[6];
2088
2089 solveImatrix(rhs_top, rhs_bot, result);
2090 for (int i = 0; i < 3; ++i) {
2091 accel_top[0][i] = -result[i];
2092 accel_bottom[0][i] = -result[i+3];
2093 }
2094
2095 }
2096
2097 static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough
2098 // now do the loop over the m_links
2099 for (int i = 0; i < num_links; ++i) {
2100 const int parent = m_links[i].m_parent;
2101
2102 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
2103 accel_top[parent+1], accel_bottom[parent+1],
2104 accel_top[i+1], accel_bottom[i+1]);
2105
2106 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2107 {
2108 const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
2109 const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
2110
2111 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
2112 }
2113
2114 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
2115 mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
2116
2117 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2118 {
2119 accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
2120 accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
2121 }
2122 }
2123
2124 // transform base accelerations back to the world frame.
2125 btVector3 omegadot_out;
2126 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
2127 output[0] = omegadot_out[0];
2128 output[1] = omegadot_out[1];
2129 output[2] = omegadot_out[2];
2130
2131 btVector3 vdot_out;
2132 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
2133
2134 output[3] = vdot_out[0];
2135 output[4] = vdot_out[1];
2136 output[5] = vdot_out[2];
2137
2138 /////////////////
2139 //printf("delta = [");
2140 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
2141 // printf("%.2f ", output[dof]);
2142 //printf("]\n");
2143 /////////////////
2144 }
2145 #else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
calcAccelerationDeltasMultiDof(const btScalar * force,btScalar * output,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v) const2146 void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
2147 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
2148 {
2149 // Temporary matrices/vectors -- use scratch space from caller
2150 // so that we don't have to keep reallocating every frame
2151
2152 int num_links = getNumLinks();
2153 scratch_r.resize(m_dofCount);
2154 scratch_v.resize(4*num_links + 4);
2155
2156 btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
2157 btVector3 * v_ptr = &scratch_v[0];
2158
2159 // zhat_i^A (scratch space)
2160 btSpatialForceVector * zeroAccSpatFrc = (btSpatialForceVector *)v_ptr;
2161 v_ptr += num_links * 2 + 2;
2162
2163 // rot_from_parent (cached from calcAccelerations)
2164 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
2165
2166 // hhat (cached), accel (scratch)
2167 // hhat is NOT stored for the base (but ahat is)
2168 const btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0);
2169 btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr;
2170 v_ptr += num_links * 2 + 2;
2171
2172 // Y_i (scratch), invD_i (cached)
2173 const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
2174 btScalar * Y = r_ptr;
2175 ////////////////
2176 //aux variables
2177 static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; better moved to buffers since it is recalced in calcAccelerationDeltasMultiDof; num_dof of btScalar would cover all bodies
2178 static btSpatialMotionVector result; //holds results of the SolveImatrix op; it is a spatial motion vector (accel)
2179 static btScalar Y_minus_hT_a[6]; //Y - h^{T} * a; it's dofx1 for each body so a single 6x1 temp is enough
2180 static btSpatialForceVector spatForceVecTemps[6]; //6 temporary spatial force vectors
2181 static btSpatialTransformationMatrix fromParent;
2182 /////////////////
2183
2184 // First 'upward' loop.
2185 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
2186
2187 // Fill in zero_acc
2188 // -- set to force/torque on the base, zero otherwise
2189 if (m_fixedBase)
2190 {
2191 zeroAccSpatFrc[0].setZero();
2192 } else
2193 {
2194 //test forces
2195 fromParent.m_rotMat = rot_from_parent[0];
2196 fromParent.transformRotationOnly(btSpatialForceVector(-force[0],-force[1],-force[2], -force[3],-force[4],-force[5]), zeroAccSpatFrc[0]);
2197 }
2198 for (int i = 0; i < num_links; ++i)
2199 {
2200 zeroAccSpatFrc[i+1].setZero();
2201 }
2202
2203 // 'Downward' loop.
2204 // (part of TreeForwardDynamics in Mirtich.)
2205 for (int i = num_links - 1; i >= 0; --i)
2206 {
2207 const int parent = m_links[i].m_parent;
2208 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
2209
2210 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2211 {
2212 Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
2213 - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1])
2214 ;
2215 }
2216
2217 btVector3 in_top, in_bottom, out_top, out_bottom;
2218 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
2219
2220 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2221 {
2222 invD_times_Y[dof] = 0.f;
2223
2224 for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
2225 {
2226 invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
2227 }
2228 }
2229
2230 // Zp += pXi * (Zi + hi*Yi/Di)
2231 spatForceVecTemps[0] = zeroAccSpatFrc[i+1];
2232
2233 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2234 {
2235 const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
2236 //
2237 spatForceVecTemps[0] += hDof * invD_times_Y[dof];
2238 }
2239
2240
2241 fromParent.transformInverse(spatForceVecTemps[0], spatForceVecTemps[1]);
2242
2243 zeroAccSpatFrc[parent+1] += spatForceVecTemps[1];
2244 }
2245
2246 // ptr to the joint accel part of the output
2247 btScalar * joint_accel = output + 6;
2248
2249
2250 // Second 'upward' loop
2251 // (part of TreeForwardDynamics in Mirtich)
2252
2253 if (m_fixedBase)
2254 {
2255 spatAcc[0].setZero();
2256 }
2257 else
2258 {
2259 solveImatrix(zeroAccSpatFrc[0], result);
2260 spatAcc[0] = -result;
2261
2262 }
2263
2264 // now do the loop over the m_links
2265 for (int i = 0; i < num_links; ++i)
2266 {
2267 const int parent = m_links[i].m_parent;
2268 fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
2269
2270 fromParent.transform(spatAcc[parent+1], spatAcc[i+1]);
2271
2272 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2273 {
2274 const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
2275 //
2276 Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i+1].dot(hDof);
2277 }
2278
2279 const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
2280 mulMatrix(const_cast<btScalar*>(invDi), Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
2281
2282 for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
2283 spatAcc[i+1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
2284 }
2285
2286 // transform base accelerations back to the world frame.
2287 btVector3 omegadot_out;
2288 omegadot_out = rot_from_parent[0].transpose() * spatAcc[0].getAngular();
2289 output[0] = omegadot_out[0];
2290 output[1] = omegadot_out[1];
2291 output[2] = omegadot_out[2];
2292
2293 btVector3 vdot_out;
2294 vdot_out = rot_from_parent[0].transpose() * spatAcc[0].getLinear();
2295 output[3] = vdot_out[0];
2296 output[4] = vdot_out[1];
2297 output[5] = vdot_out[2];
2298
2299 /////////////////
2300 //printf("delta = [");
2301 //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
2302 // printf("%.2f ", output[dof]);
2303 //printf("]\n");
2304 /////////////////
2305 }
2306 #endif
2307
2308
calcAccelerationDeltas(const btScalar * force,btScalar * output,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v) const2309 void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
2310 btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
2311 {
2312 // Temporary matrices/vectors -- use scratch space from caller
2313 // so that we don't have to keep reallocating every frame
2314 int num_links = getNumLinks();
2315 scratch_r.resize(num_links);
2316 scratch_v.resize(4*num_links + 4);
2317
2318 btScalar * r_ptr = num_links == 0 ? 0 : &scratch_r[0];
2319 btVector3 * v_ptr = &scratch_v[0];
2320
2321 // zhat_i^A (scratch space)
2322 btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
2323 btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
2324
2325 // rot_from_parent (cached from calcAccelerations)
2326 const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
2327
2328 // hhat (cached), accel (scratch)
2329 const btVector3 * h_top = num_links > 0 ? &m_vectorBuf[0] : 0;
2330 const btVector3 * h_bottom = num_links > 0 ? &m_vectorBuf[num_links] : 0;
2331 btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
2332 btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
2333
2334 // Y_i (scratch), D_i (cached)
2335 btScalar * Y = r_ptr; r_ptr += num_links;
2336 const btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
2337
2338 btAssert(num_links == 0 || r_ptr - &scratch_r[0] == scratch_r.size());
2339 btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2340
2341
2342
2343 // First 'upward' loop.
2344 // Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
2345
2346 btVector3 input_force(force[3],force[4],force[5]);
2347 btVector3 input_torque(force[0],force[1],force[2]);
2348
2349 // Fill in zero_acc
2350 // -- set to force/torque on the base, zero otherwise
2351 if (m_fixedBase)
2352 {
2353 zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
2354 } else
2355 {
2356 zero_acc_top_angular[0] = - (rot_from_parent[0] * input_force);
2357 zero_acc_bottom_linear[0] = - (rot_from_parent[0] * input_torque);
2358 }
2359 for (int i = 0; i < num_links; ++i)
2360 {
2361 zero_acc_top_angular[i+1] = zero_acc_bottom_linear[i+1] = btVector3(0,0,0);
2362 }
2363
2364 // 'Downward' loop.
2365 for (int i = num_links - 1; i >= 0; --i)
2366 {
2367 // btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
2368 Y[i] = - SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1]);
2369 Y[i] += force[6 + i]; // add joint torque
2370
2371 const int parent = m_links[i].m_parent;
2372
2373 // Zp += pXi * (Zi + hi*Yi/Di)
2374 btVector3 in_top, in_bottom, out_top, out_bottom;
2375 const btScalar Y_over_D = Y[i] / D[i];
2376 in_top = zero_acc_top_angular[i+1] + Y_over_D * h_top[i];
2377 in_bottom = zero_acc_bottom_linear[i+1] + Y_over_D * h_bottom[i];
2378 InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
2379 in_top, in_bottom, out_top, out_bottom);
2380 zero_acc_top_angular[parent+1] += out_top;
2381 zero_acc_bottom_linear[parent+1] += out_bottom;
2382 }
2383
2384 // ptr to the joint accel part of the output
2385 btScalar * joint_accel = output + 6;
2386
2387 // Second 'upward' loop
2388 if (m_fixedBase)
2389 {
2390 accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
2391 } else
2392 {
2393 btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
2394 btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
2395
2396 float result[6];
2397 solveImatrix(rhs_top,rhs_bot, result);
2398 // printf("result=%f,%f,%f,%f,%f,%f\n",result[0],result[0],result[0],result[0],result[0],result[0]);
2399
2400 for (int i = 0; i < 3; ++i) {
2401 accel_top[0][i] = -result[i];
2402 accel_bottom[0][i] = -result[i+3];
2403 }
2404
2405 }
2406
2407 // now do the loop over the m_links
2408 for (int i = 0; i < num_links; ++i) {
2409 const int parent = m_links[i].m_parent;
2410 SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
2411 accel_top[parent+1], accel_bottom[parent+1],
2412 accel_top[i+1], accel_bottom[i+1]);
2413 btScalar Y_minus_hT_a = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1]));
2414 joint_accel[i] = Y_minus_hT_a / D[i];
2415 accel_top[i+1] += joint_accel[i] * m_links[i].getAxisTop(0);
2416 accel_bottom[i+1] += joint_accel[i] * m_links[i].getAxisBottom(0);
2417 }
2418
2419 // transform base accelerations back to the world frame.
2420 btVector3 omegadot_out;
2421 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
2422 output[0] = omegadot_out[0];
2423 output[1] = omegadot_out[1];
2424 output[2] = omegadot_out[2];
2425
2426 btVector3 vdot_out;
2427 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
2428
2429 output[3] = vdot_out[0];
2430 output[4] = vdot_out[1];
2431 output[5] = vdot_out[2];
2432
2433
2434 /////////////////
2435 /*
2436 int ndof = getNumLinks() + 6;
2437 printf("test force(impulse) (%d) = [\n",ndof);
2438
2439 for (int i=0;i<ndof;i++)
2440 {
2441 printf("%.2f ", force[i]);
2442 printf("]\n");
2443 }
2444
2445 printf("delta(%d) = [",ndof);
2446 for(int dof = 0; dof < getNumLinks() + 6; ++dof)
2447 printf("%.2f ", output[dof]);
2448 printf("]\n");
2449 /////////////////
2450 */
2451
2452 //int dummy = 0;
2453 }
2454
stepPositions(btScalar dt)2455 void btMultiBody::stepPositions(btScalar dt)
2456 {
2457 int num_links = getNumLinks();
2458 // step position by adding dt * velocity
2459 btVector3 v = getBaseVel();
2460 m_basePos += dt * v;
2461
2462 // "exponential map" method for the rotation
2463 btVector3 base_omega = getBaseOmega();
2464 const btScalar omega_norm = base_omega.norm();
2465 const btScalar omega_times_dt = omega_norm * dt;
2466 const btScalar SMALL_ROTATION_ANGLE = 0.02f; // Theoretically this should be ~ pow(FLT_EPSILON,0.25) which is ~ 0.0156
2467 if (fabs(omega_times_dt) < SMALL_ROTATION_ANGLE)
2468 {
2469 const btScalar xsq = omega_times_dt * omega_times_dt; // |omega|^2 * dt^2
2470 const btScalar sin_term = dt * (xsq / 48.0f - 0.5f); // -sin(0.5*dt*|omega|) / |omega|
2471 const btScalar cos_term = 1.0f - xsq / 8.0f; // cos(0.5*dt*|omega|)
2472 m_baseQuat = m_baseQuat * btQuaternion(sin_term * base_omega[0],sin_term * base_omega[1],sin_term * base_omega[2],cos_term);
2473 } else
2474 {
2475 m_baseQuat = m_baseQuat * btQuaternion(base_omega / omega_norm,-omega_times_dt);
2476 }
2477
2478 // Make sure the quaternion represents a valid rotation.
2479 // (Not strictly necessary, but helps prevent any round-off errors from building up.)
2480 m_baseQuat.normalize();
2481
2482 // Finally we can update m_jointPos for each of the m_links
2483 for (int i = 0; i < num_links; ++i)
2484 {
2485 float jointVel = getJointVel(i);
2486 m_links[i].m_jointPos[0] += dt * jointVel;
2487 m_links[i].updateCache();
2488 }
2489 }
2490
stepPositionsMultiDof(btScalar dt,btScalar * pq,btScalar * pqd)2491 void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
2492 {
2493 int num_links = getNumLinks();
2494 // step position by adding dt * velocity
2495 //btVector3 v = getBaseVel();
2496 //m_basePos += dt * v;
2497 //
2498 btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
2499 btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
2500 //
2501 pBasePos[0] += dt * pBaseVel[0];
2502 pBasePos[1] += dt * pBaseVel[1];
2503 pBasePos[2] += dt * pBaseVel[2];
2504
2505 ///////////////////////////////
2506 //local functor for quaternion integration (to avoid error prone redundancy)
2507 struct
2508 {
2509 //"exponential map" based on btTransformUtil::integrateTransform(..)
2510 void operator() (const btVector3 &omega, btQuaternion &quat, bool baseBody, btScalar dt)
2511 {
2512 //baseBody => quat is alias and omega is global coor
2513 //!baseBody => quat is alibi and omega is local coor
2514
2515 btVector3 axis;
2516 btVector3 angvel;
2517
2518 if(!baseBody)
2519 angvel = quatRotate(quat, omega); //if quat is not m_baseQuat, it is alibi => ok
2520 else
2521 angvel = omega;
2522
2523 btScalar fAngle = angvel.length();
2524 //limit the angular motion
2525 if (fAngle * dt > ANGULAR_MOTION_THRESHOLD)
2526 {
2527 fAngle = btScalar(0.5)*SIMD_HALF_PI / dt;
2528 }
2529
2530 if ( fAngle < btScalar(0.001) )
2531 {
2532 // use Taylor's expansions of sync function
2533 axis = angvel*( btScalar(0.5)*dt-(dt*dt*dt)*(btScalar(0.020833333333))*fAngle*fAngle );
2534 }
2535 else
2536 {
2537 // sync(fAngle) = sin(c*fAngle)/t
2538 axis = angvel*( btSin(btScalar(0.5)*fAngle*dt)/fAngle );
2539 }
2540
2541 if(!baseBody)
2542 quat = btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat;
2543 else
2544 quat = quat * btQuaternion(-axis.x(),-axis.y(),-axis.z(),btCos( fAngle*dt*btScalar(0.5) ));
2545 //equivalent to: quat = (btQuaternion(axis.x(),axis.y(),axis.z(),btCos( fAngle*dt*btScalar(0.5) )) * quat.inverse()).inverse();
2546
2547 quat.normalize();
2548 }
2549 } pQuatUpdateFun;
2550 ///////////////////////////////
2551
2552 //pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
2553 //
2554 btScalar *pBaseQuat = pq ? pq : m_baseQuat;
2555 btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
2556 //
2557 static btQuaternion baseQuat; baseQuat.setValue(pBaseQuat[0], pBaseQuat[1], pBaseQuat[2], pBaseQuat[3]);
2558 static btVector3 baseOmega; baseOmega.setValue(pBaseOmega[0], pBaseOmega[1], pBaseOmega[2]);
2559 pQuatUpdateFun(baseOmega, baseQuat, true, dt);
2560 pBaseQuat[0] = baseQuat.x();
2561 pBaseQuat[1] = baseQuat.y();
2562 pBaseQuat[2] = baseQuat.z();
2563 pBaseQuat[3] = baseQuat.w();
2564
2565
2566 //printf("pBaseOmega = %.4f %.4f %.4f\n", pBaseOmega->x(), pBaseOmega->y(), pBaseOmega->z());
2567 //printf("pBaseVel = %.4f %.4f %.4f\n", pBaseVel->x(), pBaseVel->y(), pBaseVel->z());
2568 //printf("baseQuat = %.4f %.4f %.4f %.4f\n", pBaseQuat->x(), pBaseQuat->y(), pBaseQuat->z(), pBaseQuat->w());
2569
2570 if(pq)
2571 pq += 7;
2572 if(pqd)
2573 pqd += 6;
2574
2575 // Finally we can update m_jointPos for each of the m_links
2576 for (int i = 0; i < num_links; ++i)
2577 {
2578 btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
2579 btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
2580
2581 switch(m_links[i].m_jointType)
2582 {
2583 case btMultibodyLink::ePrismatic:
2584 case btMultibodyLink::eRevolute:
2585 {
2586 btScalar jointVel = pJointVel[0];
2587 pJointPos[0] += dt * jointVel;
2588 break;
2589 }
2590 case btMultibodyLink::eSpherical:
2591 {
2592 static btVector3 jointVel; jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
2593 static btQuaternion jointOri; jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
2594 pQuatUpdateFun(jointVel, jointOri, false, dt);
2595 pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
2596 break;
2597 }
2598 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
2599 case btMultibodyLink::ePlanar:
2600 {
2601 pJointPos[0] += dt * getJointVelMultiDof(i)[0];
2602
2603 btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
2604 btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
2605 pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
2606 pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
2607
2608 break;
2609 }
2610 #endif
2611 default:
2612 {
2613 }
2614
2615 }
2616
2617 m_links[i].updateCacheMultiDof(pq);
2618
2619 if(pq)
2620 pq += m_links[i].m_posVarCount;
2621 if(pqd)
2622 pqd += m_links[i].m_dofCount;
2623 }
2624 }
2625
filConstraintJacobianMultiDof(int link,const btVector3 & contact_point,const btVector3 & normal_ang,const btVector3 & normal_lin,btScalar * jac,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m) const2626 void btMultiBody::filConstraintJacobianMultiDof(int link,
2627 const btVector3 &contact_point,
2628 const btVector3 &normal_ang,
2629 const btVector3 &normal_lin,
2630 btScalar *jac,
2631 btAlignedObjectArray<btScalar> &scratch_r,
2632 btAlignedObjectArray<btVector3> &scratch_v,
2633 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
2634 {
2635 // temporary space
2636 int num_links = getNumLinks();
2637 int m_dofCount = getNumDofs();
2638 scratch_v.resize(3*num_links + 3); //(num_links + base) offsets + (num_links + base) normals_lin + (num_links + base) normals_ang
2639 scratch_m.resize(num_links + 1);
2640
2641 btVector3 * v_ptr = &scratch_v[0];
2642 btVector3 * p_minus_com_local = v_ptr; v_ptr += num_links + 1;
2643 btVector3 * n_local_lin = v_ptr; v_ptr += num_links + 1;
2644 btVector3 * n_local_ang = v_ptr; v_ptr += num_links + 1;
2645 btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2646
2647 scratch_r.resize(m_dofCount);
2648 btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
2649
2650 btMatrix3x3 * rot_from_world = &scratch_m[0];
2651
2652 const btVector3 p_minus_com_world = contact_point - m_basePos;
2653 const btVector3 &normal_lin_world = normal_lin; //convenience
2654 const btVector3 &normal_ang_world = normal_ang;
2655
2656 rot_from_world[0] = btMatrix3x3(m_baseQuat);
2657
2658 // omega coeffients first.
2659 btVector3 omega_coeffs_world;
2660 omega_coeffs_world = p_minus_com_world.cross(normal_lin_world);
2661 jac[0] = omega_coeffs_world[0] + normal_ang_world[0];
2662 jac[1] = omega_coeffs_world[1] + normal_ang_world[1];
2663 jac[2] = omega_coeffs_world[2] + normal_ang_world[2];
2664 // then v coefficients
2665 jac[3] = normal_lin_world[0];
2666 jac[4] = normal_lin_world[1];
2667 jac[5] = normal_lin_world[2];
2668
2669 //create link-local versions of p_minus_com and normal
2670 p_minus_com_local[0] = rot_from_world[0] * p_minus_com_world;
2671 n_local_lin[0] = rot_from_world[0] * normal_lin_world;
2672 n_local_ang[0] = rot_from_world[0] * normal_ang_world;
2673
2674 // Set remaining jac values to zero for now.
2675 for (int i = 6; i < 6 + m_dofCount; ++i)
2676 {
2677 jac[i] = 0;
2678 }
2679
2680 // Qdot coefficients, if necessary.
2681 if (num_links > 0 && link > -1) {
2682
2683 // TODO: speed this up -- don't calculate for m_links we don't need.
2684 // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2685 // which is resulting in repeated work being done...)
2686
2687 // calculate required normals & positions in the local frames.
2688 for (int i = 0; i < num_links; ++i) {
2689
2690 // transform to local frame
2691 const int parent = m_links[i].m_parent;
2692 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2693 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2694
2695 n_local_lin[i+1] = mtx * n_local_lin[parent+1];
2696 n_local_ang[i+1] = mtx * n_local_ang[parent+1];
2697 p_minus_com_local[i+1] = mtx * p_minus_com_local[parent+1] - m_links[i].m_cachedRVector;
2698
2699 // calculate the jacobian entry
2700 switch(m_links[i].m_jointType)
2701 {
2702 case btMultibodyLink::eRevolute:
2703 {
2704 results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
2705 results[m_links[i].m_dofOffset] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2706 break;
2707 }
2708 case btMultibodyLink::ePrismatic:
2709 {
2710 results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
2711 break;
2712 }
2713 case btMultibodyLink::eSpherical:
2714 {
2715 results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(0));
2716 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisTop(1).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(1));
2717 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisTop(2).cross(p_minus_com_local[i+1]) + m_links[i].getAxisBottom(2));
2718
2719 results[m_links[i].m_dofOffset + 0] += n_local_ang[i+1].dot(m_links[i].getAxisTop(0));
2720 results[m_links[i].m_dofOffset + 1] += n_local_ang[i+1].dot(m_links[i].getAxisTop(1));
2721 results[m_links[i].m_dofOffset + 2] += n_local_ang[i+1].dot(m_links[i].getAxisTop(2));
2722
2723 break;
2724 }
2725 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
2726 case btMultibodyLink::ePlanar:
2727 {
2728 results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
2729 results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
2730 results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
2731
2732 break;
2733 }
2734 #endif
2735 default:
2736 {
2737 }
2738 }
2739
2740 }
2741
2742 // Now copy through to output.
2743 //printf("jac[%d] = ", link);
2744 while (link != -1)
2745 {
2746 for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
2747 {
2748 jac[6 + m_links[link].m_dofOffset + dof] = results[m_links[link].m_dofOffset + dof];
2749 //printf("%.2f\t", jac[6 + m_links[link].m_dofOffset + dof]);
2750 }
2751
2752 link = m_links[link].m_parent;
2753 }
2754 //printf("]\n");
2755 }
2756 }
2757
fillContactJacobian(int link,const btVector3 & contact_point,const btVector3 & normal,btScalar * jac,btAlignedObjectArray<btScalar> & scratch_r,btAlignedObjectArray<btVector3> & scratch_v,btAlignedObjectArray<btMatrix3x3> & scratch_m) const2758 void btMultiBody::fillContactJacobian(int link,
2759 const btVector3 &contact_point,
2760 const btVector3 &normal,
2761 btScalar *jac,
2762 btAlignedObjectArray<btScalar> &scratch_r,
2763 btAlignedObjectArray<btVector3> &scratch_v,
2764 btAlignedObjectArray<btMatrix3x3> &scratch_m) const
2765 {
2766 // temporary space
2767 int num_links = getNumLinks();
2768 scratch_v.resize(2*num_links + 2);
2769 scratch_m.resize(num_links + 1);
2770
2771 btVector3 * v_ptr = &scratch_v[0];
2772 btVector3 * p_minus_com = v_ptr; v_ptr += num_links + 1;
2773 btVector3 * n_local = v_ptr; v_ptr += num_links + 1;
2774 btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
2775
2776 scratch_r.resize(num_links);
2777 btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
2778
2779 btMatrix3x3 * rot_from_world = &scratch_m[0];
2780
2781 const btVector3 p_minus_com_world = contact_point - m_basePos;
2782
2783 rot_from_world[0] = btMatrix3x3(m_baseQuat);
2784
2785 p_minus_com[0] = rot_from_world[0] * p_minus_com_world;
2786 n_local[0] = rot_from_world[0] * normal;
2787
2788 // omega coeffients first.
2789 if (this->m_fixedBase)
2790 {
2791 for (int i=0;i<6;i++)
2792 {
2793 jac[i]=0;
2794 }
2795 } else
2796 {
2797 btVector3 omega_coeffs;
2798
2799 omega_coeffs = p_minus_com_world.cross(normal);
2800 jac[0] = omega_coeffs[0];
2801 jac[1] = omega_coeffs[1];
2802 jac[2] = omega_coeffs[2];
2803 // then v coefficients
2804 jac[3] = normal[0];
2805 jac[4] = normal[1];
2806 jac[5] = normal[2];
2807 }
2808 // Set remaining jac values to zero for now.
2809 for (int i = 6; i < 6 + num_links; ++i) {
2810 jac[i] = 0;
2811 }
2812
2813 // Qdot coefficients, if necessary.
2814 if (num_links > 0 && link > -1) {
2815
2816 // TODO: speed this up -- don't calculate for m_links we don't need.
2817 // (Also, we are making 3 separate calls to this function, for the normal & the 2 friction directions,
2818 // which is resulting in repeated work being done...)
2819
2820 // calculate required normals & positions in the local frames.
2821 for (int i = 0; i < num_links; ++i) {
2822
2823 // transform to local frame
2824 const int parent = m_links[i].m_parent;
2825 const btMatrix3x3 mtx(m_links[i].m_cachedRotParentToThis);
2826 rot_from_world[i+1] = mtx * rot_from_world[parent+1];
2827
2828 n_local[i+1] = mtx * n_local[parent+1];
2829 p_minus_com[i+1] = mtx * p_minus_com[parent+1] - m_links[i].m_cachedRVector;
2830
2831 // calculate the jacobian entry
2832 if (m_links[i].m_jointType == btMultibodyLink::eRevolute) {
2833 results[i] = n_local[i+1].dot( m_links[i].getAxisTop(0).cross(p_minus_com[i+1]) + m_links[i].getAxisBottom(0) );
2834 } else {
2835 results[i] = n_local[i+1].dot( m_links[i].getAxisBottom(0) );
2836 }
2837 }
2838
2839 // Now copy through to output.
2840 //printf("jac[%d] = ", link);
2841 while (link != -1)
2842 {
2843 jac[6 + link] = results[link];
2844 //printf("%.2f\t", jac[6 + link]);
2845 link = m_links[link].m_parent;
2846 }
2847 //printf("]\n");
2848 }
2849 }
2850
wakeUp()2851 void btMultiBody::wakeUp()
2852 {
2853 m_awake = true;
2854 }
2855
goToSleep()2856 void btMultiBody::goToSleep()
2857 {
2858 m_awake = false;
2859 }
2860
checkMotionAndSleepIfRequired(btScalar timestep)2861 void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
2862 {
2863 int num_links = getNumLinks();
2864 extern bool gDisableDeactivation;
2865 if (!m_canSleep || gDisableDeactivation)
2866 {
2867 m_awake = true;
2868 m_sleepTimer = 0;
2869 return;
2870 }
2871
2872 // motion is computed as omega^2 + v^2 + (sum of squares of joint velocities)
2873 btScalar motion = 0;
2874 if(m_isMultiDof)
2875 {
2876 for (int i = 0; i < 6 + m_dofCount; ++i)
2877 motion += m_realBuf[i] * m_realBuf[i];
2878 }
2879 else
2880 {
2881 for (int i = 0; i < 6 + num_links; ++i)
2882 motion += m_realBuf[i] * m_realBuf[i];
2883 }
2884
2885
2886 if (motion < SLEEP_EPSILON) {
2887 m_sleepTimer += timestep;
2888 if (m_sleepTimer > SLEEP_TIMEOUT) {
2889 goToSleep();
2890 }
2891 } else {
2892 m_sleepTimer = 0;
2893 if (!m_awake)
2894 wakeUp();
2895 }
2896 }
2897