Bullet Collision Detection & Physics Library
btMultiBody.h
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 #ifndef BT_MULTIBODY_H
25 #define BT_MULTIBODY_H
26 
27 #include "LinearMath/btScalar.h"
28 #include "LinearMath/btVector3.h"
30 #include "LinearMath/btMatrix3x3.h"
32 
33 #ifdef BT_USE_DOUBLE_PRECISION
34  #define btMultiBodyData btMultiBodyDoubleData
35  #define btMultiBodyDataName "btMultiBodyDoubleData"
36  #define btMultiBodyLinkData btMultiBodyLinkDoubleData
37  #define btMultiBodyLinkDataName "btMultiBodyLinkDoubleData"
38 #else
39  #define btMultiBodyData btMultiBodyFloatData
40  #define btMultiBodyDataName "btMultiBodyFloatData"
41  #define btMultiBodyLinkData btMultiBodyLinkFloatData
42  #define btMultiBodyLinkDataName "btMultiBodyLinkFloatData"
43 #endif //BT_USE_DOUBLE_PRECISION
44 
45 #include "btMultiBodyLink.h"
47 
49 {
50 public:
51 
52 
54 
55  //
56  // initialization
57  //
58 
59  btMultiBody(int n_links, // NOT including the base
60  btScalar mass, // mass of base
61  const btVector3 &inertia, // inertia of base, in base frame; assumed diagonal
62  bool fixedBase, // whether the base is fixed (true) or can move (false)
63  bool canSleep,
64  bool multiDof = false
65  );
66 
67 
68  virtual ~btMultiBody();
69 
70  void setupFixed(int linkIndex,
71  btScalar mass,
72  const btVector3 &inertia,
73  int parent,
74  const btQuaternion &rotParentToThis,
75  const btVector3 &parentComToThisPivotOffset,
76  const btVector3 &thisPivotToThisComOffset,
77  bool disableParentCollision);
78 
79 
80  void setupPrismatic(int i,
81  btScalar mass,
82  const btVector3 &inertia,
83  int parent,
84  const btQuaternion &rotParentToThis,
85  const btVector3 &jointAxis,
86  const btVector3 &parentComToThisPivotOffset,
87  const btVector3 &thisPivotToThisComOffset,
88  bool disableParentCollision);
89 
90  void setupRevolute(int linkIndex, // 0 to num_links-1
91  btScalar mass,
92  const btVector3 &inertia,
93  int parentIndex,
94  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
95  const btVector3 &jointAxis, // in my frame
96  const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
97  const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
98  bool disableParentCollision=false);
99 
100  void setupSpherical(int linkIndex, // 0 to num_links-1
101  btScalar mass,
102  const btVector3 &inertia,
103  int parent,
104  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
105  const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
106  const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
107  bool disableParentCollision=false);
108 
109  void setupPlanar(int i, // 0 to num_links-1
110  btScalar mass,
111  const btVector3 &inertia,
112  int parent,
113  const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
114  const btVector3 &rotationAxis,
115  const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
116  bool disableParentCollision=false);
117 
118  const btMultibodyLink& getLink(int index) const
119  {
120  return m_links[index];
121  }
122 
124  {
125  return m_links[index];
126  }
127 
128 
129  void setBaseCollider(btMultiBodyLinkCollider* collider)//collider can be NULL to disable collision for the base
130  {
131  m_baseCollider = collider;
132  }
134  {
135  return m_baseCollider;
136  }
138  {
139  return m_baseCollider;
140  }
141 
142  //
143  // get parent
144  // input: link num from 0 to num_links-1
145  // output: link num from 0 to num_links-1, OR -1 to mean the base.
146  //
147  int getParent(int link_num) const;
148 
149 
150  //
151  // get number of m_links, masses, moments of inertia
152  //
153 
154  int getNumLinks() const { return m_links.size(); }
155  int getNumDofs() const { return m_dofCount; }
156  int getNumPosVars() const { return m_posVarCnt; }
157  btScalar getBaseMass() const { return m_baseMass; }
158  const btVector3 & getBaseInertia() const { return m_baseInertia; }
159  btScalar getLinkMass(int i) const;
160  const btVector3 & getLinkInertia(int i) const;
161 
162 
163 
164  //
165  // change mass (incomplete: can only change base mass and inertia at present)
166  //
167 
168  void setBaseMass(btScalar mass) { m_baseMass = mass; }
169  void setBaseInertia(const btVector3 &inertia) { m_baseInertia = inertia; }
170 
171 
172  //
173  // get/set pos/vel/rot/omega for the base link
174  //
175 
176  const btVector3 & getBasePos() const { return m_basePos; } // in world frame
177  const btVector3 getBaseVel() const
178  {
179  return btVector3(m_realBuf[3],m_realBuf[4],m_realBuf[5]);
180  } // in world frame
182  {
183  return m_baseQuat;
184  } // rotates world vectors into base frame
185  btVector3 getBaseOmega() const { return btVector3(m_realBuf[0],m_realBuf[1],m_realBuf[2]); } // in world frame
186 
187  void setBasePos(const btVector3 &pos)
188  {
189  m_basePos = pos;
190  }
191 
193  {
194  setBasePos(tr.getOrigin());
196 
197  }
198 
200  {
201  btTransform tr;
202  tr.setOrigin(getBasePos());
204  return tr;
205  }
206 
207  void setBaseVel(const btVector3 &vel)
208  {
209 
210  m_realBuf[3]=vel[0]; m_realBuf[4]=vel[1]; m_realBuf[5]=vel[2];
211  }
212  void setWorldToBaseRot(const btQuaternion &rot)
213  {
214  m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
215  }
216  void setBaseOmega(const btVector3 &omega)
217  {
218  m_realBuf[0]=omega[0];
219  m_realBuf[1]=omega[1];
220  m_realBuf[2]=omega[2];
221  }
222 
223 
224  //
225  // get/set pos/vel for child m_links (i = 0 to num_links-1)
226  //
227 
228  btScalar getJointPos(int i) const;
229  btScalar getJointVel(int i) const;
230 
231  btScalar * getJointVelMultiDof(int i);
232  btScalar * getJointPosMultiDof(int i);
233 
234  const btScalar * getJointVelMultiDof(int i) const ;
235  const btScalar * getJointPosMultiDof(int i) const ;
236 
237  void setJointPos(int i, btScalar q);
238  void setJointVel(int i, btScalar qdot);
239  void setJointPosMultiDof(int i, btScalar *q);
240  void setJointVelMultiDof(int i, btScalar *qdot);
241 
242 
243 
244  //
245  // direct access to velocities as a vector of 6 + num_links elements.
246  // (omega first, then v, then joint velocities.)
247  //
248  const btScalar * getVelocityVector() const
249  {
250  return &m_realBuf[0];
251  }
252 /* btScalar * getVelocityVector()
253  {
254  return &real_buf[0];
255  }
256  */
257 
258  //
259  // get the frames of reference (positions and orientations) of the child m_links
260  // (i = 0 to num_links-1)
261  //
262 
263  const btVector3 & getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
264  const btQuaternion & getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
265 
266 
267  //
268  // transform vectors in local frame of link i to world frame (or vice versa)
269  //
270  btVector3 localPosToWorld(int i, const btVector3 &vec) const;
271  btVector3 localDirToWorld(int i, const btVector3 &vec) const;
272  btVector3 worldPosToLocal(int i, const btVector3 &vec) const;
273  btVector3 worldDirToLocal(int i, const btVector3 &vec) const;
274 
275 
276  //
277  // calculate kinetic energy and angular momentum
278  // useful for debugging.
279  //
280 
281  btScalar getKineticEnergy() const;
283 
284 
285  //
286  // set external forces and torques. Note all external forces/torques are given in the WORLD frame.
287  //
288 
289  void clearForcesAndTorques();
290  void clearConstraintForces();
291 
292  void clearVelocities();
293 
294  void addBaseForce(const btVector3 &f)
295  {
296  m_baseForce += f;
297  }
298  void addBaseTorque(const btVector3 &t) { m_baseTorque += t; }
299  void addLinkForce(int i, const btVector3 &f);
300  void addLinkTorque(int i, const btVector3 &t);
301 
303  {
305  }
307  void addLinkConstraintForce(int i, const btVector3 &f);
308  void addLinkConstraintTorque(int i, const btVector3 &t);
309 
310 
311 void addJointTorque(int i, btScalar Q);
312  void addJointTorqueMultiDof(int i, int dof, btScalar Q);
313  void addJointTorqueMultiDof(int i, const btScalar *Q);
314 
315  const btVector3 & getBaseForce() const { return m_baseForce; }
316  const btVector3 & getBaseTorque() const { return m_baseTorque; }
317  const btVector3 & getLinkForce(int i) const;
318  const btVector3 & getLinkTorque(int i) const;
319  btScalar getJointTorque(int i) const;
321 
322 
323  //
324  // dynamics routines.
325  //
326 
327  // timestep the velocities (given the external forces/torques set using addBaseForce etc).
328  // also sets up caches for calcAccelerationDeltas.
329  //
330  // Note: the caller must provide three vectors which are used as
331  // temporary scratch space. The idea here is to reduce dynamic
332  // memory allocation: the same scratch vectors can be re-used
333  // again and again for different Multibodies, instead of each
334  // btMultiBody allocating (and then deallocating) their own
335  // individual scratch buffers. This gives a considerable speed
336  // improvement, at least on Windows (where dynamic memory
337  // allocation appears to be fairly slow).
338  //
339  void stepVelocities(btScalar dt,
343 
348  bool isConstraintPass=false
349  );
350 
351  // calcAccelerationDeltas
352  // input: force vector (in same format as jacobian, i.e.:
353  // 3 torque values, 3 force values, num_links joint torque values)
354  // output: 3 omegadot values, 3 vdot values, num_links q_double_dot values
355  // (existing contents of output array are replaced)
356  // stepVelocities must have been called first.
357  void calcAccelerationDeltas(const btScalar *force, btScalar *output,
359  btAlignedObjectArray<btVector3> &scratch_v) const;
360 
363  btAlignedObjectArray<btVector3> &scratch_v) const;
364 
365  // apply a delta-vee directly. used in sequential impulses code.
366  void applyDeltaVee(const btScalar * delta_vee)
367  {
368 
369  for (int i = 0; i < 6 + getNumLinks(); ++i)
370  {
371  m_realBuf[i] += delta_vee[i];
372  }
373 
374  }
375  void applyDeltaVee(const btScalar * delta_vee, btScalar multiplier)
376  {
377  btScalar sum = 0;
378  for (int i = 0; i < 6 + getNumLinks(); ++i)
379  {
380  sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
381  }
382  btScalar l = btSqrt(sum);
383  /*
384  static btScalar maxl = -1e30f;
385  if (l>maxl)
386  {
387  maxl=l;
388  // printf("maxl=%f\n",maxl);
389  }
390  */
391  if (l>m_maxAppliedImpulse)
392  {
393 // printf("exceeds 100: l=%f\n",maxl);
394  multiplier *= m_maxAppliedImpulse/l;
395  }
396 
397  for (int i = 0; i < 6 + getNumLinks(); ++i)
398  {
399  sum += delta_vee[i]*multiplier*delta_vee[i]*multiplier;
400  m_realBuf[i] += delta_vee[i] * multiplier;
402  }
403  }
404 
405  void applyDeltaVeeMultiDof2(const btScalar * delta_vee, btScalar multiplier)
406  {
407  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
408  {
409  m_deltaV[dof] += delta_vee[dof] * multiplier;
410  }
411  }
413  {
415 
416  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
417  {
418  m_deltaV[dof] = 0.f;
419  }
420  }
421 
422  void applyDeltaVeeMultiDof(const btScalar * delta_vee, btScalar multiplier)
423  {
424  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
425  // printf("%.4f ", delta_vee[dof]*multiplier);
426  //printf("\n");
427 
428  //btScalar sum = 0;
429  //for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
430  //{
431  // sum += delta_vee[dof]*multiplier*delta_vee[dof]*multiplier;
432  //}
433  //btScalar l = btSqrt(sum);
434 
435  //if (l>m_maxAppliedImpulse)
436  //{
437  // multiplier *= m_maxAppliedImpulse/l;
438  //}
439 
440  for (int dof = 0; dof < 6 + getNumDofs(); ++dof)
441  {
442  m_realBuf[dof] += delta_vee[dof] * multiplier;
444  }
445  }
446 
447 
448 
449  // timestep the positions (given current velocities).
450  void stepPositions(btScalar dt);
451  void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
452 
453 
454  //
455  // contacts
456  //
457 
458  // This routine fills out a contact constraint jacobian for this body.
459  // the 'normal' supplied must be -n for body1 or +n for body2 of the contact.
460  // 'normal' & 'contact_point' are both given in world coordinates.
461  void fillContactJacobian(int link,
462  const btVector3 &contact_point,
463  const btVector3 &normal,
464  btScalar *jac,
467  btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
468 
469  //multidof version of fillContactJacobian
471  const btVector3 &contact_point,
472  const btVector3 &normal,
473  btScalar *jac,
476  btAlignedObjectArray<btMatrix3x3> &scratch_m) const { filConstraintJacobianMultiDof(link, contact_point, btVector3(0, 0, 0), normal, jac, scratch_r, scratch_v, scratch_m); }
477 
478  //a more general version of fillContactJacobianMultiDof which does not assume..
479  //.. that the constraint in question is contact or, to be more precise, constrains linear velocity only
480  void filConstraintJacobianMultiDof(int link,
481  const btVector3 &contact_point,
482  const btVector3 &normal_ang,
483  const btVector3 &normal_lin,
484  btScalar *jac,
487  btAlignedObjectArray<btMatrix3x3> &scratch_m) const;
488 
489 
490  //
491  // sleeping
492  //
493  void setCanSleep(bool canSleep)
494  {
495  m_canSleep = canSleep;
496  }
497 
498  bool getCanSleep()const
499  {
500  return m_canSleep;
501  }
502 
503  bool isAwake() const { return m_awake; }
504  void wakeUp();
505  void goToSleep();
507 
508  bool hasFixedBase() const
509  {
510  return m_fixedBase;
511  }
512 
513  int getCompanionId() const
514  {
515  return m_companionId;
516  }
517  void setCompanionId(int id)
518  {
519  //printf("for %p setCompanionId(%d)\n",this, id);
520  m_companionId = id;
521  }
522 
523  void setNumLinks(int numLinks)//careful: when changing the number of m_links, make sure to re-initialize or update existing m_links
524  {
525  m_links.resize(numLinks);
526  }
527 
529  {
530  return m_linearDamping;
531  }
533  {
534  m_linearDamping = damp;
535  }
537  {
538  return m_angularDamping;
539  }
541  {
542  m_angularDamping = damp;
543  }
544 
545  bool getUseGyroTerm() const
546  {
547  return m_useGyroTerm;
548  }
549  void setUseGyroTerm(bool useGyro)
550  {
551  m_useGyroTerm = useGyro;
552  }
554  {
555  return m_maxCoordinateVelocity ;
556  }
558  {
559  m_maxCoordinateVelocity = maxVel;
560  }
561 
563  {
564  return m_maxAppliedImpulse;
565  }
567  {
568  m_maxAppliedImpulse = maxImp;
569  }
571  {
573  }
574  bool hasSelfCollision() const
575  {
576  return m_hasSelfCollision;
577  }
578 
579  bool isMultiDof() { return m_isMultiDof; }
580  void finalizeMultiDof();
581 
582  void useRK4Integration(bool use) { m_useRK4 = use; }
583  bool isUsingRK4Integration() const { return m_useRK4; }
584  void useGlobalVelocities(bool use) { m_useGlobalVelocities = use; }
586 
587  bool isPosUpdated() const
588  {
589  return __posUpdated;
590  }
591  void setPosUpdated(bool updated)
592  {
593  __posUpdated = updated;
594  }
595 
596  //internalNeedsJointFeedback is for internal use only
598  {
600  }
602 
604 
605  virtual int calculateSerializeBufferSize() const;
606 
608  virtual const char* serialize(void* dataBuffer, class btSerializer* serializer) const;
609 
610  const char* getBaseName() const
611  {
612  return m_baseName;
613  }
615  void setBaseName(const char* name)
616  {
617  m_baseName = name;
618  }
619 
620 private:
621  btMultiBody(const btMultiBody &); // not implemented
622  void operator=(const btMultiBody &); // not implemented
623 
624  void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
625 
626  void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
627  void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
628 
630  {
631  int dofOffset = 0, cfgOffset = 0;
632  for(int bidx = 0; bidx < m_links.size(); ++bidx)
633  {
634  m_links[bidx].m_dofOffset = dofOffset; m_links[bidx].m_cfgOffset = cfgOffset;
635  dofOffset += m_links[bidx].m_dofCount; cfgOffset += m_links[bidx].m_posVarCount;
636  }
637  }
638 
639  void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
640 
641 
642 private:
643 
645  const char* m_baseName;//memory needs to be manager by user!
646 
647  btVector3 m_basePos; // position of COM of base (world frame)
648  btQuaternion m_baseQuat; // rotates world points into base frame
649 
650  btScalar m_baseMass; // mass of the base
651  btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)
652 
653  btVector3 m_baseForce; // external force applied to base. World frame.
654  btVector3 m_baseTorque; // external torque applied to base. World frame.
655 
656  btVector3 m_baseConstraintForce; // external force applied to base. World frame.
657  btVector3 m_baseConstraintTorque; // external torque applied to base. World frame.
658 
659  btAlignedObjectArray<btMultibodyLink> m_links; // array of m_links, excluding the base. index from 0 to num_links-1.
661 
662 
663  //
664  // realBuf:
665  // offset size array
666  // 0 6 + num_links v (base_omega; base_vel; joint_vels) MULTIDOF [sysdof x sysdof for D matrices (TOO MUCH!) + pos_delta which is sys-cfg sized]
667  // 6+num_links num_links D
668  //
669  // vectorBuf:
670  // offset size array
671  // 0 num_links h_top
672  // num_links num_links h_bottom
673  //
674  // matrixBuf:
675  // offset size array
676  // 0 num_links+1 rot_from_parent
677  //
682 
683 
688 
690 
691  // Sleep parameters.
692  bool m_awake;
695 
707 
710 };
711 
713 {
719 
720 
721  char *m_linkName;
722  char *m_jointName;
724 
725  btVector3DoubleData m_linkInertia; // inertia of the base (in local frame; diagonal)
726  double m_linkMass;
729 
730 
731 
732 
735  double m_jointPos[7];
736  double m_jointVel[6];
737  double m_jointTorque[6];
738 
739 
740 
741 };
742 
744 {
750 
751 
752  char *m_linkName;
753  char *m_jointName;
755 
756  btVector3FloatData m_linkInertia; // inertia of the base (in local frame; diagonal)
758  float m_linkMass;
761 
762 
763 
764  float m_jointPos[7];
765  float m_jointVel[6];
766  float m_jointTorque[6];
768 
769 
770 };
771 
774 {
775  char *m_baseName;
778 
780  btVector3DoubleData m_baseInertia; // inertia of the base (in local frame; diagonal)
781 
783  double m_baseMass;
784 
785  char m_padding[4];
786 
787 };
788 
791 {
792  char *m_baseName;
796  btVector3FloatData m_baseInertia; // inertia of the base (in local frame; diagonal)
797 
798  float m_baseMass;
800 };
801 
802 
803 
804 #endif
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
static T sum(const btAlignedObjectArray< T > &items)
void setupPlanar(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &rotationAxis, const btVector3 &parentComToThisComOffset, bool disableParentCollision=false)
void stepVelocitiesMultiDof(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m, bool isConstraintPass=false)
void setCanSleep(bool canSleep)
Definition: btMultiBody.h:493
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
void setupFixed(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void clearConstraintForces()
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< btMultiBodyLinkCollider * > m_colliders
Definition: btMultiBody.h:660
btAlignedObjectArray< btMatrix3x3 > m_matrixBuf
Definition: btMultiBody.h:681
bool m_useGyroTerm
Definition: btMultiBody.h:699
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
virtual ~btMultiBody()
BT_DECLARE_ALIGNED_ALLOCATOR()
btVector3 m_baseForce
Definition: btMultiBody.h:653
bool isUsingGlobalVelocities() const
Definition: btMultiBody.h:585
const btVector3 getBaseVel() const
Definition: btMultiBody.h:177
btMultiBodyLinkCollider * getBaseCollider()
Definition: btMultiBody.h:137
void setNumLinks(int numLinks)
Definition: btMultiBody.h:523
btQuaternion getRotation() const
Return a quaternion representing the rotation.
Definition: btTransform.h:122
btMultiBodyLinkCollider * m_baseCollider
Definition: btMultiBody.h:644
btScalar m_baseMass
Definition: btMultiBody.h:650
void finalizeMultiDof()
bool m_fixedBase
Definition: btMultiBody.h:689
bool isAwake() const
Definition: btMultiBody.h:503
These spatial algebra classes are used for btMultiBody, see BulletDynamics/Featherstone.
int getNumLinks() const
Definition: btMultiBody.h:154
btVector3DoubleData m_jointAxisTop[6]
Definition: btMultiBody.h:717
void stepPositionsMultiDof(btScalar dt, btScalar *pq=0, btScalar *pqd=0)
btScalar m_maxCoordinateVelocity
Definition: btMultiBody.h:701
btVector3FloatData m_jointAxisTop[6]
Definition: btMultiBody.h:748
void addBaseTorque(const btVector3 &t)
Definition: btMultiBody.h:298
btTransform getBaseWorldTransform() const
Definition: btMultiBody.h:199
const btVector3 & getLinkForce(int i) const
virtual int calculateSerializeBufferSize() const
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
Definition: btMultiBody.h:773
btScalar getBaseMass() const
Definition: btMultiBody.h:157
btScalar btSqrt(btScalar y)
Definition: btScalar.h:418
void setupSpherical(int linkIndex, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision=false)
btQuaternionDoubleData m_zeroRotParentToThis
Definition: btMultiBody.h:714
btVector3 localDirToWorld(int i, const btVector3 &vec) const
void addLinkConstraintForce(int i, const btVector3 &f)
btMatrix3x3 m_cachedInertiaLowerRight
Definition: btMultiBody.h:687
btCollisionObjectFloatData * m_linkCollider
Definition: btMultiBody.h:754
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 setBaseCollider(btMultiBodyLinkCollider *collider)
Definition: btMultiBody.h:129
bool internalNeedsJointFeedback() const
Definition: btMultiBody.h:597
btScalar getJointPos(int i) const
void btClamp(T &a, const T &lb, const T &ub)
Definition: btMinMax.h:59
const btVector3 & getBaseTorque() const
Definition: btMultiBody.h:316
bool isPosUpdated() const
Definition: btMultiBody.h:587
void setJointPosMultiDof(int i, btScalar *q)
void updateCollisionObjectWorldTransforms(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btVector3 m_baseConstraintTorque
Definition: btMultiBody.h:657
void applyDeltaVeeMultiDof(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:422
void setWorldToBaseRot(const btQuaternion &rot)
Definition: btMultiBody.h:212
btQuaternion inverse(const btQuaternion &q)
Return the inverse of a quaternion.
Definition: btQuaternion.h:867
btScalar m_maxAppliedImpulse
Definition: btMultiBody.h:700
const char * getBaseName() const
Definition: btMultiBody.h:610
btAlignedObjectArray< btScalar > m_deltaV
Definition: btMultiBody.h:678
btScalar getMaxAppliedImpulse() const
Definition: btMultiBody.h:562
void applyDeltaVee(const btScalar *delta_vee)
Definition: btMultiBody.h:366
void clearForcesAndTorques()
btVector3 getAngularMomentum() const
btMultiBody(int n_links, btScalar mass, const btVector3 &inertia, bool fixedBase, bool canSleep, bool multiDof=false)
Definition: btMultiBody.cpp:90
void setBaseVel(const btVector3 &vel)
Definition: btMultiBody.h:207
btVector3 m_baseTorque
Definition: btMultiBody.h:654
bool m_isMultiDof
Definition: btMultiBody.h:703
void addLinkConstraintTorque(int i, const btVector3 &t)
int getNumPosVars() const
Definition: btMultiBody.h:156
void forwardKinematics(btAlignedObjectArray< btQuaternion > &scratch_q, btAlignedObjectArray< btVector3 > &scratch_m)
btMatrix3x3 m_cachedInertiaTopLeft
Definition: btMultiBody.h:684
int m_companionId
Definition: btMultiBody.h:696
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
const btVector3 & getBaseInertia() const
Definition: btMultiBody.h:158
void setupPrismatic(int i, btScalar mass, const btVector3 &inertia, int parent, const btQuaternion &rotParentToThis, const btVector3 &jointAxis, const btVector3 &parentComToThisPivotOffset, const btVector3 &thisPivotToThisComOffset, bool disableParentCollision)
void setHasSelfCollision(bool hasSelfCollision)
Definition: btMultiBody.h:570
void setMaxCoordinateVelocity(btScalar maxVel)
Definition: btMultiBody.h:557
int size() const
return the number of elements in the array
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btTransformFloatData m_baseWorldTransform
Definition: btMultiBody.h:795
void setUseGyroTerm(bool useGyro)
Definition: btMultiBody.h:549
btQuaternion m_baseQuat
Definition: btMultiBody.h:648
btMatrix3x3 m_cachedInertiaLowerLeft
Definition: btMultiBody.h:686
const btVector3 & getLinkTorque(int i) const
void addLinkTorque(int i, const btVector3 &t)
void setCompanionId(int id)
Definition: btMultiBody.h:517
void setBaseOmega(const btVector3 &omega)
Definition: btMultiBody.h:216
bool m_hasSelfCollision
Definition: btMultiBody.h:702
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
bool hasFixedBase() const
Definition: btMultiBody.h:508
btMultibodyLink & getLink(int index)
Definition: btMultiBody.h:123
#define output
void setRotation(const btQuaternion &q)
Set the rotational element by btQuaternion.
Definition: btTransform.h:165
void addJointTorque(int i, btScalar Q)
btScalar getLinearDamping() const
Definition: btMultiBody.h:528
void solveImatrix(const btVector3 &rhs_top, const btVector3 &rhs_bot, float result[6]) const
const btVector3 & getRVector(int i) const
bool hasSelfCollision() const
Definition: btMultiBody.h:574
btQuaternionFloatData m_zeroRotParentToThis
Definition: btMultiBody.h:745
btQuaternion inverse() const
Return the inverse of this quaternion.
Definition: btQuaternion.h:453
void setJointVel(int i, btScalar qdot)
btVector3FloatData m_baseInertia
Definition: btMultiBody.h:796
void operator=(const btMultiBody &)
btScalar m_sleepTimer
Definition: btMultiBody.h:694
void setJointPos(int i, btScalar q)
btVector3FloatData m_jointAxisBottom[6]
Definition: btMultiBody.h:749
void clearVelocities()
btVector3 m_basePos
Definition: btMultiBody.h:647
void setAngularDamping(btScalar damp)
Definition: btMultiBody.h:540
btVector3FloatData m_thisPivotToThisComOffset
Definition: btMultiBody.h:747
void checkMotionAndSleepIfRequired(btScalar timestep)
btVector3DoubleData m_baseInertia
Definition: btMultiBody.h:780
btVector3 m_baseConstraintForce
Definition: btMultiBody.h:656
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
bool m_internalNeedsJointFeedback
the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal ...
Definition: btMultiBody.h:709
btVector3 m_baseInertia
Definition: btMultiBody.h:651
void setLinearDamping(btScalar damp)
Definition: btMultiBody.h:532
const char * m_baseName
Definition: btMultiBody.h:645
btScalar getLinkMass(int i) const
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
void addBaseForce(const btVector3 &f)
Definition: btMultiBody.h:294
btAlignedObjectArray< btScalar > m_realBuf
Definition: btMultiBody.h:679
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
void updateLinksDofOffsets()
Definition: btMultiBody.h:629
void setMaxAppliedImpulse(btScalar maxImp)
Definition: btMultiBody.h:566
btVector3 getBaseOmega() const
Definition: btMultiBody.h:185
btMultiBodyLinkFloatData * m_links
Definition: btMultiBody.h:793
int getCompanionId() const
Definition: btMultiBody.h:513
void fillContactJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
Definition: btMultiBody.h:470
bool m_useGlobalVelocities
Definition: btMultiBody.h:706
btScalar m_linearDamping
Definition: btMultiBody.h:697
btScalar getAngularDamping() const
Definition: btMultiBody.h:536
void resize(int newsize, const T &fillData=T())
void addJointTorqueMultiDof(int i, int dof, btScalar Q)
for serialization
Definition: btTransform.h:253
btCollisionObjectDoubleData * m_linkCollider
Definition: btMultiBody.h:723
void goToSleep()
void applyDeltaVee(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:375
void setBaseInertia(const btVector3 &inertia)
Definition: btMultiBody.h:169
void applyDeltaVeeMultiDof2(const btScalar *delta_vee, btScalar multiplier)
Definition: btMultiBody.h:405
btTransformDoubleData m_baseWorldTransform
Definition: btMultiBody.h:779
bool getCanSleep() const
Definition: btMultiBody.h:498
void setPosUpdated(bool updated)
Definition: btMultiBody.h:591
btVector3 worldPosToLocal(int i, const btVector3 &vec) const
const btQuaternion & getParentToLocalRot(int i) const
btVector3DoubleData m_parentComToThisComOffset
Definition: btMultiBody.h:715
bool isUsingRK4Integration() const
Definition: btMultiBody.h:583
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:680
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64 ...
Definition: btMultiBody.h:790
void setJointVelMultiDof(int i, btScalar *qdot)
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:133
btVector3 worldDirToLocal(int i, const btVector3 &vec) const
bool isMultiDof()
Definition: btMultiBody.h:579
const btVector3 & getLinkInertia(int i) const
btVector3FloatData m_parentComToThisComOffset
Definition: btMultiBody.h:746
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
btScalar getJointTorque(int i) const
const btQuaternion & getWorldToBaseRot() const
Definition: btMultiBody.h:181
void addBaseConstraintForce(const btVector3 &f)
Definition: btMultiBody.h:302
void setBaseMass(btScalar mass)
Definition: btMultiBody.h:168
void stepPositions(btScalar dt)
int getParent(int link_num) const
btVector3FloatData m_linkInertia
Definition: btMultiBody.h:756
bool __posUpdated
Definition: btMultiBody.h:704
const btVector3 & getBasePos() const
Definition: btMultiBody.h:176
btVector3 localPosToWorld(int i, const btVector3 &vec) const
const btVector3 & getBaseForce() const
Definition: btMultiBody.h:315
btAlignedObjectArray< btMultibodyLink > m_links
Definition: btMultiBody.h:659
btScalar * getJointTorqueMultiDof(int i)
btVector3DoubleData m_linkInertia
Definition: btMultiBody.h:725
void setBasePos(const btVector3 &pos)
Definition: btMultiBody.h:187
btVector3DoubleData m_thisPivotToThisComOffset
Definition: btMultiBody.h:716
void addBaseConstraintTorque(const btVector3 &t)
Definition: btMultiBody.h:306
void useGlobalVelocities(bool use)
Definition: btMultiBody.h:584
btMultiBodyLinkDoubleData * m_links
Definition: btMultiBody.h:776
btCollisionObjectFloatData * m_baseCollider
Definition: btMultiBody.h:794
btMatrix3x3 m_cachedInertiaTopRight
Definition: btMultiBody.h:685
int getNumDofs() const
Definition: btMultiBody.h:155
btScalar * getJointPosMultiDof(int i)
void useRK4Integration(bool use)
Definition: btMultiBody.h:582
virtual const char * serialize(void *dataBuffer, class btSerializer *serializer) const
fills the dataBuffer and returns the struct name (and 0 on failure)
void setBaseWorldTransform(const btTransform &tr)
Definition: btMultiBody.h:192
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:248
void processDeltaVeeMultiDof2()
Definition: btMultiBody.h:412
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:698
bool getUseGyroTerm() const
Definition: btMultiBody.h:545
btVector3DoubleData m_jointAxisBottom[6]
Definition: btMultiBody.h:718
void stepVelocities(btScalar dt, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m)
btScalar getMaxCoordinateVelocity() const
Definition: btMultiBody.h:553
btScalar getJointVel(int i) const
bool m_canSleep
Definition: btMultiBody.h:693
btScalar getKineticEnergy() const
void setBaseName(const char *name)
memory of setBaseName needs to be manager by user
Definition: btMultiBody.h:615
btCollisionObjectDoubleData * m_baseCollider
Definition: btMultiBody.h:777