Bullet
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 {
26 };
27 
28 //both defines are now permanently enabled
29 #define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
30 #define TEST_SPATIAL_ALGEBRA_LAYER
31 
32 //
33 // Various spatial helper functions
34 //
35 
36 //namespace {
37 
38 
40 
41 //}
42 
43 //
44 // Link struct
45 //
46 
48 {
49 
51 
52  btScalar m_mass; // mass of link
53  btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
54 
55  int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
56 
57  btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
58 
59  btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
60  //this is set to zero for planar joint (see also m_eVector comment)
61 
62  // m_eVector is constant, but depends on the joint type:
63  // revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
64  // planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
65  // todo: fix the planar so it is consistent with the other joints
66 
68 
70 
72  {
73  eRevolute = 0,
76  ePlanar = 3,
77  eFixed = 4,
79  };
80 
81 
82 
83  // "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
84  // for prismatic: m_axesTop[0] = zero;
85  // m_axesBottom[0] = unit vector along the joint axis.
86  // for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
87  // m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
88  //
89  // for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
90  // m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
91  //
92  // for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
93  // m_axesTop[1][2] = zero
94  // m_axesBottom[0] = zero
95  // m_axesBottom[1][2] = unit vectors along the translational axes on that plane
97  void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
98  void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
99  void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_topVec.setValue(x, y, z); }
100  void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
101  const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
102  const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
103 
105 
106  btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
107  btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
108 
109  btVector3 m_appliedForce; // In WORLD frame
110  btVector3 m_appliedTorque; // In WORLD frame
111 
114 
116 
117  //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
118  //It gets set to zero after each internal stepSimulation call
120 
122  int m_flags;
123 
124 
125  int m_dofCount, m_posVarCount; //redundant but handy
126 
128 
130 
131  btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
132 
133  const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
134  const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
135 
136  // ctor: set some sensible defaults
138  : m_mass(1),
139  m_parent(-1),
140  m_zeroRotParentToThis(0, 0, 0, 1),
141  m_cachedRotParentToThis(0, 0, 0, 1),
142  m_collider(0),
143  m_flags(0),
144  m_dofCount(0),
145  m_posVarCount(0),
146  m_jointType(btMultibodyLink::eInvalid),
147  m_jointFeedback(0),
148  m_linkName(0),
149  m_jointName(0)
150  {
151 
152  m_inertiaLocal.setValue(1, 1, 1);
153  setAxisTop(0, 0., 0., 0.);
154  setAxisBottom(0, 1., 0., 0.);
155  m_dVector.setValue(0, 0, 0);
156  m_eVector.setValue(0, 0, 0);
157  m_cachedRVector.setValue(0, 0, 0);
158  m_appliedForce.setValue( 0, 0, 0);
159  m_appliedTorque.setValue(0, 0, 0);
160  //
161  m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
162  m_jointPos[3] = 1.f; //"quat.w"
163  m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
164  m_cachedWorldTransform.setIdentity();
165  }
166 
167  // routine to update m_cachedRotParentToThis and m_cachedRVector
168  void updateCache()
169  {
170  //multidof
171  if (m_jointType == eRevolute)
172  {
173  m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-m_jointPos[0]) * m_zeroRotParentToThis;
174  m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
175  } else
176  {
177  // m_cachedRotParentToThis never changes, so no need to update
178  m_cachedRVector = m_eVector + m_jointPos[0] * getAxisBottom(0);
179  }
180  }
181 
183  {
184  btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
185 
186  switch(m_jointType)
187  {
188  case eRevolute:
189  {
190  m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
191  m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
192 
193  break;
194  }
195  case ePrismatic:
196  {
197  // m_cachedRotParentToThis never changes, so no need to update
198  m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
199 
200  break;
201  }
202  case eSpherical:
203  {
204  m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
205  m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
206 
207  break;
208  }
209  case ePlanar:
210  {
211  m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
212  m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
213 
214  break;
215  }
216  case eFixed:
217  {
218  m_cachedRotParentToThis = m_zeroRotParentToThis;
219  m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
220 
221  break;
222  }
223  default:
224  {
225  //invalid type
226  btAssert(0);
227  }
228  }
229  }
230 };
231 
232 
233 #endif //BT_MULTIBODY_LINK_H
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:113
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:884
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:55
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278