Bullet Collision Detection & Physics Library: btMultiBodyLink.h Source File

Bullet Collision Detection & Physics Library

btMultiBodyLink.h
Go to the documentation of this file.
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
19#include "LinearMath/btQuaternion.h"
20#include "LinearMath/btVector3.h"
21#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
22
23 enum btMultiBodyLinkFlags
24{
25 BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
26 BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
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
39#include "LinearMath/btSpatialAlgebra.h"
40
41//}
42
43//
44// Link struct
45//
46
47 struct btMultibodyLink
48{
49 BT_DECLARE_ALIGNED_ALLOCATOR();
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
66 btVector3 m_eVector;
67
68 btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
69
70 enum eFeatherstoneJointType
71 {
72 eRevolute = 0,
73 ePrismatic = 1,
74 eSpherical = 2,
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
93 btSpatialMotionVector m_axes[6];
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
110 int m_dofOffset, m_cfgOffset;
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
122 btVector3 m_appliedConstraintForce; // In WORLD frame
123 btVector3 m_appliedConstraintTorque; // In WORLD frame
124
125 btScalar m_jointPos[7];
126 btScalar m_jointPos_interpolate[7];
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
130 btScalar m_jointTorque[6];
131
132 class btMultiBodyLinkCollider *m_collider;
133 int m_flags;
134
135 int m_dofCount, m_posVarCount; //redundant but handy
136
137 eFeatherstoneJointType m_jointType;
138
139 struct btMultiBodyJointFeedback *m_jointFeedback;
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
155 btMultibodyLink()
156 : m_mass(1),
157 m_parent(-1),
158 m_zeroRotParentToThis(0, 0, 0, 1),
159 m_cachedRotParentToThis(0, 0, 0, 1),
160 m_cachedRotParentToThis_interpolate(0, 0, 0, 1),
161 m_collider(0),
162 m_flags(0),
163 m_dofCount(0),
164 m_posVarCount(0),
165 m_jointType(btMultibodyLink::eInvalid),
166 m_jointFeedback(0),
167 m_linkName(0),
168 m_jointName(0),
169 m_userPtr(0),
170 m_jointDamping(0),
171 m_jointFriction(0),
172 m_jointLowerLimit(0),
173 m_jointUpperLimit(0),
174 m_jointMaxForce(0),
175 m_jointMaxVelocity(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"
191 m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
192 m_cachedWorldTransform.setIdentity();
193 }
194
195 // routine to update m_cachedRotParentToThis and m_cachedRVector
196 void updateCacheMultiDof(btScalar *pq = 0)
197 {
198 btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
199 btQuaternion& cachedRot = m_cachedRotParentToThis;
200 btVector3& cachedVector = m_cachedRVector;
201 switch (m_jointType)
202 {
203 case eRevolute:
204 {
205 cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
206 cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
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 }
244 m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis;
245 m_cachedRVector_interpolate = m_cachedRVector;
246 }
247
248 void updateInterpolationCacheMultiDof()
249 {
250 btScalar *pJointPos = &m_jointPos_interpolate[0];
251
252 btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate;
253 btVector3& cachedVector = m_cachedRVector_interpolate;
254 switch (m_jointType)
255 {
256 case eRevolute:
257 {
258 cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
259 cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
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
301};
302
303#endif //BT_MULTIBODY_LINK_H
btCollisionObject.h
btMultiBodyLinkFlags
btMultiBodyLinkFlags
Definition btMultiBodyLink.h:24
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION
@ BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION
Definition btMultiBodyLink.h:26
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
@ BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
Definition btMultiBodyLink.h:25
btQuaternion.h
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition btQuaternion.h:926
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition btScalar.h:314
btAssert
#define btAssert(x)
Definition btScalar.h:153
btSpatialAlgebra.h
btVector3.h
btMultiBodyLinkCollider
Definition btMultiBodyLinkCollider.h:33
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition btQuaternion.h:50
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.
Definition btVector3.h:82
btMultiBodyJointFeedback
Definition btMultiBodyJointFeedback.h:21
btMultibodyLink::m_jointFriction
btScalar m_jointFriction
Definition btMultiBodyLink.h:148
btMultibodyLink::m_zeroRotParentToThis
btQuaternion m_zeroRotParentToThis
Definition btMultiBodyLink.h:56
btMultibodyLink::m_jointPos_interpolate
btScalar m_jointPos_interpolate[7]
Definition btMultiBodyLink.h:126
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition btMultiBodyLink.h:132
btMultibodyLink::m_appliedConstraintForce
btVector3 m_appliedConstraintForce
Definition btMultiBodyLink.h:122
btMultibodyLink::setAxisBottom
void setAxisBottom(int dof, const btVector3 &axis)
Definition btMultiBodyLink.h:95
btMultibodyLink::m_cachedRotParentToThis
btQuaternion m_cachedRotParentToThis
Definition btMultiBodyLink.h:112
btMultibodyLink::getAxisTop
const btVector3 & getAxisTop(int dof) const
Definition btMultiBodyLink.h:107
btMultibodyLink::setAxisTop
void setAxisTop(int dof, const btVector3 &axis)
Definition btMultiBodyLink.h:94
btMultibodyLink::m_cachedRotParentToThis_interpolate
btQuaternion m_cachedRotParentToThis_interpolate
Definition btMultiBodyLink.h:116
btMultibodyLink::m_appliedTorque
btVector3 m_appliedTorque
Definition btMultiBodyLink.h:120
btMultibodyLink::m_inertiaLocal
btVector3 m_inertiaLocal
Definition btMultiBodyLink.h:52
btMultibodyLink::m_jointUpperLimit
btScalar m_jointUpperLimit
Definition btMultiBodyLink.h:150
btMultibodyLink::updateInterpolationCacheMultiDof
void updateInterpolationCacheMultiDof()
Definition btMultiBodyLink.h:248
btMultibodyLink::getAxisBottom
const btVector3 & getAxisBottom(int dof) const
Definition btMultiBodyLink.h:108
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition btMultiBodyLink.h:137
btMultibodyLink::m_dofCount
int m_dofCount
Definition btMultiBodyLink.h:135
btMultibodyLink::m_jointLowerLimit
btScalar m_jointLowerLimit
Definition btMultiBodyLink.h:149
btMultibodyLink::eFeatherstoneJointType
eFeatherstoneJointType
Definition btMultiBodyLink.h:71
btMultibodyLink::eInvalid
@ eInvalid
Definition btMultiBodyLink.h:77
btMultibodyLink::ePlanar
@ ePlanar
Definition btMultiBodyLink.h:75
btMultibodyLink::ePrismatic
@ ePrismatic
Definition btMultiBodyLink.h:73
btMultibodyLink::eRevolute
@ eRevolute
Definition btMultiBodyLink.h:72
btMultibodyLink::eSpherical
@ eSpherical
Definition btMultiBodyLink.h:74
btMultibodyLink::eFixed
@ eFixed
Definition btMultiBodyLink.h:76
btMultibodyLink::m_absFrameTotVelocity
btSpatialMotionVector m_absFrameTotVelocity
Definition btMultiBodyLink.h:68
btMultibodyLink::m_mass
btScalar m_mass
Definition btMultiBodyLink.h:51
btMultibodyLink::BT_DECLARE_ALIGNED_ALLOCATOR
BT_DECLARE_ALIGNED_ALLOCATOR()
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition btMultiBodyLink.h:141
btMultibodyLink::m_jointMaxForce
btScalar m_jointMaxForce
Definition btMultiBodyLink.h:151
btMultibodyLink::setAxisBottom
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
Definition btMultiBodyLink.h:103
btMultibodyLink::m_flags
int m_flags
Definition btMultiBodyLink.h:133
btMultibodyLink::m_eVector
btVector3 m_eVector
Definition btMultiBodyLink.h:66
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition btMultiBodyLink.h:110
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition btMultiBodyLink.h:196
btMultibodyLink::m_appliedConstraintTorque
btVector3 m_appliedConstraintTorque
Definition btMultiBodyLink.h:123
btMultibodyLink::m_jointMaxVelocity
btScalar m_jointMaxVelocity
Definition btMultiBodyLink.h:152
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition btMultiBodyLink.h:125
btMultibodyLink::m_jointFeedback
struct btMultiBodyJointFeedback * m_jointFeedback
Definition btMultiBodyLink.h:139
btMultibodyLink::m_appliedForce
btVector3 m_appliedForce
Definition btMultiBodyLink.h:119
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition btMultiBodyLink.h:58
btMultibodyLink::m_jointTorque
btScalar m_jointTorque[6]
Definition btMultiBodyLink.h:130
btMultibodyLink::m_absFrameLocVelocity
btSpatialMotionVector m_absFrameLocVelocity
Definition btMultiBodyLink.h:68
btMultibodyLink::btMultibodyLink
btMultibodyLink()
Definition btMultiBodyLink.h:155
btMultibodyLink::m_cachedRVector_interpolate
btVector3 m_cachedRVector_interpolate
Definition btMultiBodyLink.h:117
btMultibodyLink::m_cachedRVector
btVector3 m_cachedRVector
Definition btMultiBodyLink.h:113
btMultibodyLink::m_jointDamping
btScalar m_jointDamping
Definition btMultiBodyLink.h:147
btMultibodyLink::m_parent
int m_parent
Definition btMultiBodyLink.h:54
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition btMultiBodyLink.h:110
btMultibodyLink::m_userPtr
const void * m_userPtr
Definition btMultiBodyLink.h:145
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition btMultiBodyLink.h:135
btMultibodyLink::m_jointName
const char * m_jointName
Definition btMultiBodyLink.h:144
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition btMultiBodyLink.h:93
btMultibodyLink::m_linkName
const char * m_linkName
Definition btMultiBodyLink.h:143
btMultibodyLink::setAxisTop
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
Definition btMultiBodyLink.h:99
btSpatialMotionVector
Definition btSpatialAlgebra.h:95