Bullet Collision Detection & Physics Library
btMultiBody.cpp
Go to the documentation of this file.
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"
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 {
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 
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 
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 
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 
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 
132 {
133 }
134 
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 {
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)
159  //
160  m_links[i].updateCacheMultiDof();
161 
162  //
163  //if(m_isMultiDof)
164  // resizeInternalMultiDofBuffers();
165  //
167 
168 }
169 
170 
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)
205  //
206  if(m_isMultiDof)
207  m_links[i].updateCacheMultiDof();
208  else
209  m_links[i].updateCache();
210  //
211  if(m_isMultiDof)
213 }
214 
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)
248  //
249  if(m_isMultiDof)
250  m_links[i].updateCacheMultiDof();
251  else
252  m_links[i].updateCache();
253  //
254  if(m_isMultiDof)
256 }
257 
258 
259 
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 {
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)
295  //
296  m_links[i].updateCacheMultiDof();
297  //
299 }
300 
301 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
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 {
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)
345  //
346  m_links[i].updateCacheMultiDof();
347  //
349 }
350 #endif
351 
353 {
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 
360 }
361 
362 int btMultiBody::getParent(int i) const
363 {
364  return m_links[i].m_parent;
365 }
366 
368 {
369  return m_links[i].m_mass;
370 }
371 
373 {
374  return m_links[i].m_inertiaLocal;
375 }
376 
378 {
379  return m_links[i].m_jointPos[0];
380 }
381 
383 {
384  return m_realBuf[6 + i];
385 }
386 
388 {
389  return &m_links[i].m_jointPos[0];
390 }
391 
393 {
394  return &m_realBuf[6 + m_links[i].m_dofOffset];
395 }
396 
398 {
399  m_links[i].m_jointPos[0] = q;
400  m_links[i].updateCache();
401 }
402 
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 
412 {
413  m_realBuf[6 + i] = qdot;
414 }
415 
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 
422 const btVector3 & btMultiBody::getRVector(int i) const
423 {
424  return m_links[i].m_cachedRVector;
425 }
426 
428 {
429  return m_links[i].m_cachedRotParentToThis;
430 }
431 
432 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 
449 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 
460 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 
471 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 
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 
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 
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 
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 
556 {
557  for (int i = 0; i < 6 + getNumLinks(); ++i)
558  {
559  m_realBuf[i] = 0.f;
560  }
561 }
563 {
564  m_links[i].m_appliedForce += f;
565 }
566 
568 {
569  m_links[i].m_appliedTorque += t;
570 }
571 
573 {
574  m_links[i].m_jointTorque[0] += Q;
575 }
576 
578 {
579  m_links[i].m_jointTorque[dof] += Q;
580 }
581 
583 {
584  for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
585  m_links[i].m_jointTorque[dof] = Q[dof];
586 }
587 
589 {
590  return m_links[i].m_appliedForce;
591 }
592 
594 {
595  return m_links[i].m_appliedTorque;
596 }
597 
599 {
600  return m_links[i].m_jointTorque[0];
601 }
602 
604 {
605  return &m_links[i].m_jointTorque[0];
606 }
607 
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
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];
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]);
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 
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  {
848  {
849  invDi[0] = 1.0f / D[0];
850  break;
851  }
853 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
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 
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");
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");
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
1079  btAlignedObjectArray<btScalar> &scratch_r,
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)
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();
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
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());
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  {
1323  {
1324  invDi[0] = 1.0f / D[0];
1325  break;
1326  }
1328 #ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
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;
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 
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");
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");
1483 
1484  // Final step: add the accelerations (times dt) to the velocities.
1485  if(dt > 0.)
1486  applyDeltaVeeMultiDof(output, dt);
1487 
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  //}
1514 
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
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 
1553  btAlignedObjectArray<btScalar> &scratch_r,
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 
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");
1842  //printf("qd = [");
1843  //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1844  // printf("%.2f ", m_realBuf[dof]);
1845  //printf("]\n");
1847  //printf("qdd = [");
1848  //for(int dof = 0; dof < getNumLinks() + 6; ++dof)
1849  // printf("%.2f ", output[dof]);
1850  //printf("]\n");
1852 
1853  // Final step: add the accelerations (times dt) to the velocities.
1854  applyDeltaVee(output, dt);
1855 
1856 
1857 }
1858 
1859 void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const
1860 {
1861  int num_links = getNumLinks();
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  {
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
1909 {
1910  int num_links = getNumLinks();
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  {
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 
1949 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
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;
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 
2139  //printf("delta = [");
2140  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
2141  // printf("%.2f ", output[dof]);
2142  //printf("]\n");
2144 }
2145 #else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
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;
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;
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 
2300  //printf("delta = [");
2301  //for(int dof = 0; dof < getNumDofs() + 6; ++dof)
2302  // printf("%.2f ", output[dof]);
2303  //printf("]\n");
2305 }
2306 #endif
2307 
2308 
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 
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");
2450 */
2451 
2452  //int dummy = 0;
2453 }
2454 
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.)
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 
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 
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
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;
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  {
2585  {
2586  btScalar jointVel = pJointVel[0];
2587  pJointPos[0] += dt * jointVel;
2588  break;
2589  }
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
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 
2627  const btVector3 &contact_point,
2628  const btVector3 &normal_ang,
2629  const btVector3 &normal_lin,
2630  btScalar *jac,
2631  btAlignedObjectArray<btScalar> &scratch_r,
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  {
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  }
2709  {
2710  results[m_links[i].m_dofOffset] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(0));
2711  break;
2712  }
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
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 
2759  const btVector3 &contact_point,
2760  const btVector3 &normal,
2761  btScalar *jac,
2762  btAlignedObjectArray<btScalar> &scratch_r,
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 
2852 {
2853  m_awake = true;
2854 }
2855 
2857 {
2858  m_awake = false;
2859 }
2860 
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 }
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1046
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void setupRevolute(int linkIndex, btScalar mass, const btVector3 &inertia, int parentIndex, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:598
bool m_useGyroTerm
Definition: btMultiBody.h:617
btVector3 m_baseForce
Definition: btMultiBody.h:573
const btVector3 getBaseVel() const
Definition: btMultiBody.h:167
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btScalar m_baseMass
Definition: btMultiBody.h:570
void finalizeMultiDof()
bool m_fixedBase
Definition: btMultiBody.h:607
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
btScalar btSin(btScalar x)
Definition: btScalar.h:451
int getNumLinks() const
Definition: btMultiBody.h:145
void transformInverseRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
#define vecMulVecTranspose(v0, v1Transposed)
const btScalar & z() const
Return the z value.
Definition: btQuadWord.h:120
bool gDisableDeactivation
Definition: btRigidBody.cpp:26
const btVector3 & getLinkForce(int i) const
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
#define btAssert(x)
Definition: btScalar.h:113
btVector3 localDirToWorld(int i, const btVector3 &vec) const
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:605
btScalar * getJointVelMultiDof(int i)
void addLinkForce(int i, const btVector3 &f)
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
void addLinear(const btVector3 &linear)
btScalar getJointPos(int i) const
const btScalar & y() const
Return the y value.
Definition: btQuadWord.h:118
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisComOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
const btVector3 & getAngular() const
void setJointPosMultiDof(int i, btScalar *q)
void symmetricSpatialOuterProduct(const SpatialVectorType &a, const SpatialVectorType &b, btSymmetricSpatialDyad &out)
btScalar dot(const btSpatialForceVector &b) const
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:367
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:849
const btScalar & w() const
Return the w value.
Definition: btQuadWord.h:122
#define SIMD_HALF_PI
Definition: btScalar.h:478
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
void applyDeltaVee(const btScalar *delta_vee)
Definition: btMultiBody.h:328
btMatrix3x3 outerProduct(const btVector3 &v0, const btVector3 &v1)
void clearForcesAndTorques()
const btScalar & x() const
Return the x value.
Definition: btVector3.h:575
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:866
btVector3 getAngularMomentum() const
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool multiDof=false)
Definition: btMultiBody.cpp:84
btVector3 m_baseTorque
Definition: btMultiBody.h:574
#define ANGULAR_MOTION_THRESHOLD
bool m_isMultiDof
Definition: btMultiBody.h:621
void addAngular(const btVector3 &angular)
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:602
void transformRotationOnly(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
int size() const
return the number of elements in the array
void setVector(const btVector3 &angular, const btVector3 &linear)
btQuaternion m_baseQuat
Definition: btMultiBody.h:568
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:604
btQuaternion & normalize()
Normalize the quaternion Such that x^2 + y^2 + z^2 +w^2 = 1.
Definition: btQuaternion.h:332
const btVector3 & getLinkTorque(int i) const
void addLinkTorque(int i, const btVector3 &t)
void 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) const
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const
#define output
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Set x,y,z and zero w.
Definition: btQuadWord.h:152
void setLinear(const btVector3 &linear)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
void addJointTorque(int i, btScalar Q)
void setValue(const btScalar &xx, const btScalar &xy, const btScalar &xz, const btScalar &yx, const btScalar &yy, const btScalar &yz, const btScalar &zx, const btScalar &zy, const btScalar &zz)
Set the values of the matrix explicitly (row major)
Definition: btMatrix3x3.h:198
void setZero()
Definition: btVector3.h:671
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, float result[6]) const
const btVector3 & getRVector(int i) const
btScalar length() const
Return the length of the vector.
Definition: btVector3.h:263
void setJointVel(int i, btScalar qdot)
btScalar m_sleepTimer
Definition: btMultiBody.h:612
void setAngular(const btVector3 &angular)
void setJointPos(int i, btScalar q)
void cross(const SpatialVectorType &b, SpatialVectorType &out) const
void clearVelocities()
btScalar norm() const
Return the norm (length) of the vector.
Definition: btVector3.h:269
btVector3 m_basePos
Definition: btMultiBody.h:567
const btScalar & y() const
Return the y value.
Definition: btVector3.h:577
void checkMotionAndSleepIfRequired(btScalar timestep)
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btVector3 m_baseInertia
Definition: btMultiBody.h:571
btScalar getLinkMass(int i) const
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:596
void setMatrix(const btMatrix3x3 &topLeftMat, const btMatrix3x3 &topRightMat, const btMatrix3x3 &bottomLeftMat)
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void updateLinksDofOffsets()
Definition: btMultiBody.h:550
btVector3 getBaseOmega() const
Definition: btMultiBody.h:175
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:952
bool m_useGlobalVelocities
Definition: btMultiBody.h:624
btScalar m_linearDamping
Definition: btMultiBody.h:615
void resize(int newsize, const T &fillData=T())
const btVector3 & getLinear() const
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
void goToSleep()
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:1001
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
const btQuaternion & getParentToLocalRot(int i) const
const btVector3 & getAngular() const
void setVector(const btVector3 &angular, const btVector3 &linear)
void transformInverse(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
void fillContactJacobian(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
btAlignedObjectArray< btVector3 > m_vectorBuf
Definition: btMultiBody.h:597
const btScalar & x() const
Return the x value.
Definition: btQuadWord.h:116
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
void setJointVelMultiDof(int i, btScalar *qdot)
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
const btVector3 & getLinkInertia(int i) const
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:48
void addVector(const btVector3 &angular, const btVector3 &linear)
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar getJointTorque(int i) const
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:171
void stepPositions(btScalar dt)
int getParent(int link_num) const
const btVector3 & getBasePos() const
Definition: btMultiBody.h:166
btVector3 localPosToWorld(int i, const btVector3 &vec) const
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:576
btScalar * getJointTorqueMultiDof(int i)
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:603
int getNumDofs() const
Definition: btMultiBody.h:146
btScalar * getJointPosMultiDof(int i)
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btScalar m_angularDamping
Definition: btMultiBody.h:616
btScalar btCos(btScalar x)
Definition: btScalar.h:450
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar getJointVel(int i) const
bool m_canSleep
Definition: btMultiBody.h:611
void transform(const SpatialVectorType &inVec, SpatialVectorType &outVec, eOutputOperation outOp=None)
btScalar getKineticEnergy() const
const btScalar & z() const
Return the z value.
Definition: btVector3.h:579
const btVector3 & getLinear() const