Blender  V3.3
btMultiBodyLink.h
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 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.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
16 #ifndef BT_MULTIBODY_LINK_H
17 #define BT_MULTIBODY_LINK_H
18 
20 #include "LinearMath/btVector3.h"
22 
24 {
27 };
28 
29 //both defines are now permanently enabled
30 #define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
31 #define TEST_SPATIAL_ALGEBRA_LAYER
32 
33 //
34 // Various spatial helper functions
35 //
36 
37 //namespace {
38 
40 
41 //}
42 
43 //
44 // Link struct
45 //
46 
48 {
50 
51  btScalar m_mass; // mass of link
52  btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
53 
54  int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
55 
56  btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
57 
58  btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
59  //this is set to zero for planar joint (see also m_eVector comment)
60 
61  // m_eVector is constant, but depends on the joint type:
62  // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
63  // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
64  // todo: fix the planar so it is consistent with the other joints
65 
67 
69 
71  {
72  eRevolute = 0,
75  ePlanar = 3,
76  eFixed = 4,
77  eInvalid
78  };
79 
80  // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
81  // for prismatic: m_axesTop[0] = zero;
82  // m_axesBottom[0] = unit vector along the joint axis.
83  // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
84  // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
85  //
86  // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
87  // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
88  //
89  // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
90  // m_axesTop[1][2] = zero
91  // m_axesBottom[0] = zero
92  // m_axesBottom[1][2] = unit vectors along the translational axes on that plane
94  void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
95  void setAxisBottom(int dof, const btVector3 &axis)
96  {
97  m_axes[dof].m_bottomVec = axis;
98  }
99  void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
100  {
101  m_axes[dof].m_topVec.setValue(x, y, z);
102  }
103  void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
104  {
105  m_axes[dof].m_bottomVec.setValue(x, y, z);
106  }
107  const btVector3 &getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
108  const btVector3 &getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
109 
111 
112  btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
113  btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
114 
115  // predicted verstion
116  btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame
117  btVector3 m_cachedRVector_interpolate; // vector from COM of parent to COM of this link, in local frame.
118 
119  btVector3 m_appliedForce; // In WORLD frame
120  btVector3 m_appliedTorque; // In WORLD frame
121 
124 
127 
128  //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
129  //It gets set to zero after each internal stepSimulation call
131 
133  int m_flags;
134 
135  int m_dofCount, m_posVarCount; //redundant but handy
136 
138 
140 
141  btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics
142 
143  const char *m_linkName; //m_linkName memory needs to be managed by the developer/user!
144  const char *m_jointName; //m_jointName memory needs to be managed by the developer/user!
145  const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user!
146 
147  btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
148  btScalar m_jointFriction; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual friction using a velocity motor.
149  btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
150  btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
151  btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
152  btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
153 
154  // ctor: set some sensible defaults
156  : m_mass(1),
157  m_parent(-1),
158  m_zeroRotParentToThis(0, 0, 0, 1),
159  m_cachedRotParentToThis(0, 0, 0, 1),
161  m_collider(0),
162  m_flags(0),
163  m_dofCount(0),
164  m_posVarCount(0),
166  m_jointFeedback(0),
167  m_linkName(0),
168  m_jointName(0),
169  m_userPtr(0),
170  m_jointDamping(0),
171  m_jointFriction(0),
174  m_jointMaxForce(0),
176  {
177  m_inertiaLocal.setValue(1, 1, 1);
178  setAxisTop(0, 0., 0., 0.);
179  setAxisBottom(0, 1., 0., 0.);
180  m_dVector.setValue(0, 0, 0);
181  m_eVector.setValue(0, 0, 0);
182  m_cachedRVector.setValue(0, 0, 0);
183  m_cachedRVector_interpolate.setValue(0, 0, 0);
184  m_appliedForce.setValue(0, 0, 0);
185  m_appliedTorque.setValue(0, 0, 0);
186  m_appliedConstraintForce.setValue(0, 0, 0);
187  m_appliedConstraintTorque.setValue(0, 0, 0);
188  //
189  m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
190  m_jointPos[3] = 1.f; //"quat.w"
192  m_cachedWorldTransform.setIdentity();
193  }
194 
195  // routine to update m_cachedRotParentToThis and m_cachedRVector
197  {
198  btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
200  btVector3& cachedVector = m_cachedRVector;
201  switch (m_jointType)
202  {
203  case eRevolute:
204  {
205  cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
207 
208  break;
209  }
210  case ePrismatic:
211  {
212  // m_cachedRotParentToThis never changes, so no need to update
213  cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
214 
215  break;
216  }
217  case eSpherical:
218  {
219  cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
220  cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
221 
222  break;
223  }
224  case ePlanar:
225  {
226  cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
227  cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
228 
229  break;
230  }
231  case eFixed:
232  {
233  cachedRot = m_zeroRotParentToThis;
234  cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
235 
236  break;
237  }
238  default:
239  {
240  //invalid type
241  btAssert(0);
242  }
243  }
246  }
247 
249  {
250  btScalar *pJointPos = &m_jointPos_interpolate[0];
251 
253  btVector3& cachedVector = m_cachedRVector_interpolate;
254  switch (m_jointType)
255  {
256  case eRevolute:
257  {
258  cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
260 
261  break;
262  }
263  case ePrismatic:
264  {
265  // m_cachedRotParentToThis never changes, so no need to update
266  cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
267 
268  break;
269  }
270  case eSpherical:
271  {
272  cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
273  cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
274 
275  break;
276  }
277  case ePlanar:
278  {
279  cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
280  cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector);
281 
282  break;
283  }
284  case eFixed:
285  {
286  cachedRot = m_zeroRotParentToThis;
287  cachedVector = m_dVector + quatRotate(cachedRot, m_eVector);
288 
289  break;
290  }
291  default:
292  {
293  //invalid type
294  btAssert(0);
295  }
296  }
297  }
298 };
299 
300 #endif //BT_MULTIBODY_LINK_H
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint GLsizei GLsizei GLenum type _GL_VOID_RET _GL_VOID GLsizei GLenum GLenum const void *pixels _GL_VOID_RET _GL_VOID const void *pointer _GL_VOID_RET _GL_VOID GLdouble v _GL_VOID_RET _GL_VOID GLfloat v _GL_VOID_RET _GL_VOID GLint GLint i2 _GL_VOID_RET _GL_VOID GLint j _GL_VOID_RET _GL_VOID GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble GLdouble GLdouble zFar _GL_VOID_RET _GL_UINT GLdouble *equation _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLenum GLfloat *v _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLfloat *values _GL_VOID_RET _GL_VOID GLushort *values _GL_VOID_RET _GL_VOID GLenum GLfloat *params _GL_VOID_RET _GL_VOID GLenum GLdouble *params _GL_VOID_RET _GL_VOID GLenum GLint *params _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_BOOL GLfloat param _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLushort pattern _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLint GLdouble GLdouble GLint GLint const GLdouble *points _GL_VOID_RET _GL_VOID GLdouble GLdouble u2 _GL_VOID_RET _GL_VOID GLdouble GLdouble GLint GLdouble GLdouble v2 _GL_VOID_RET _GL_VOID GLenum GLfloat param _GL_VOID_RET _GL_VOID GLenum GLint param _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLdouble GLdouble nz _GL_VOID_RET _GL_VOID GLfloat GLfloat nz _GL_VOID_RET _GL_VOID GLint GLint nz _GL_VOID_RET _GL_VOID GLshort GLshort nz _GL_VOID_RET _GL_VOID GLsizei const void *pointer _GL_VOID_RET _GL_VOID GLsizei const GLfloat *values _GL_VOID_RET _GL_VOID GLsizei const GLushort *values _GL_VOID_RET _GL_VOID GLint param _GL_VOID_RET _GL_VOID const GLuint const GLclampf *priorities _GL_VOID_RET _GL_VOID GLdouble y _GL_VOID_RET _GL_VOID GLfloat y _GL_VOID_RET _GL_VOID GLint y _GL_VOID_RET _GL_VOID GLshort y _GL_VOID_RET _GL_VOID GLdouble GLdouble z _GL_VOID_RET _GL_VOID GLfloat GLfloat z _GL_VOID_RET _GL_VOID GLint GLint z _GL_VOID_RET _GL_VOID GLshort GLshort z _GL_VOID_RET _GL_VOID GLdouble GLdouble z
_GL_VOID GLfloat value _GL_VOID_RET _GL_VOID const GLuint GLboolean *residences _GL_BOOL_RET _GL_VOID GLsizei GLfloat GLfloat GLfloat GLfloat const GLubyte *bitmap _GL_VOID_RET _GL_VOID GLenum const void *lists _GL_VOID_RET _GL_VOID const GLdouble *equation _GL_VOID_RET _GL_VOID GLdouble GLdouble blue _GL_VOID_RET _GL_VOID GLfloat GLfloat blue _GL_VOID_RET _GL_VOID GLint GLint blue _GL_VOID_RET _GL_VOID GLshort GLshort blue _GL_VOID_RET _GL_VOID GLubyte GLubyte blue _GL_VOID_RET _GL_VOID GLuint GLuint blue _GL_VOID_RET _GL_VOID GLushort GLushort blue _GL_VOID_RET _GL_VOID GLbyte GLbyte GLbyte alpha _GL_VOID_RET _GL_VOID GLdouble GLdouble GLdouble alpha _GL_VOID_RET _GL_VOID GLfloat GLfloat GLfloat alpha _GL_VOID_RET _GL_VOID GLint GLint GLint alpha _GL_VOID_RET _GL_VOID GLshort GLshort GLshort alpha _GL_VOID_RET _GL_VOID GLubyte GLubyte GLubyte alpha _GL_VOID_RET _GL_VOID GLuint GLuint GLuint alpha _GL_VOID_RET _GL_VOID GLushort GLushort GLushort alpha _GL_VOID_RET _GL_VOID GLenum mode _GL_VOID_RET _GL_VOID GLint y
SIMD_FORCE_INLINE btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define btAssert(x)
Definition: btScalar.h:295
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:30
btVector3
btVector3 can be used to represent 3D points and vectors. It has an un-used w component to suit 16-by...
Definition: btVector3.h:82
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:50