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

Bullet Collision Detection & Physics Library

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
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 btVector3 m_appliedForce; // In WORLD frame
116 btVector3 m_appliedTorque; // In WORLD frame
117
118 btVector3 m_appliedConstraintForce; // In WORLD frame
119 btVector3 m_appliedConstraintTorque; // In WORLD frame
120
121 btScalar m_jointPos[7];
122
123 //m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
124 //It gets set to zero after each internal stepSimulation call
125 btScalar m_jointTorque[6];
126
127 class btMultiBodyLinkCollider *m_collider;
128 int m_flags;
129
130 int m_dofCount, m_posVarCount; //redundant but handy
131
132 eFeatherstoneJointType m_jointType;
133
134 struct btMultiBodyJointFeedback *m_jointFeedback;
135
136 btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics
137
138 const char *m_linkName; //m_linkName memory needs to be managed by the developer/user!
139 const char *m_jointName; //m_jointName memory needs to be managed by the developer/user!
140 const void *m_userPtr; //m_userPtr ptr needs to be managed by the developer/user!
141
142 btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
143 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.
144 btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
145 btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
146 btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
147 btScalar m_jointMaxVelocity; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
148
149 // ctor: set some sensible defaults
150 btMultibodyLink()
151 : m_mass(1),
152 m_parent(-1),
153 m_zeroRotParentToThis(0, 0, 0, 1),
154 m_cachedRotParentToThis(0, 0, 0, 1),
155 m_collider(0),
156 m_flags(0),
157 m_dofCount(0),
158 m_posVarCount(0),
159 m_jointType(btMultibodyLink::eInvalid),
160 m_jointFeedback(0),
161 m_linkName(0),
162 m_jointName(0),
163 m_userPtr(0),
164 m_jointDamping(0),
165 m_jointFriction(0),
166 m_jointLowerLimit(0),
167 m_jointUpperLimit(0),
168 m_jointMaxForce(0),
169 m_jointMaxVelocity(0)
170 {
171 m_inertiaLocal.setValue(1, 1, 1);
172 setAxisTop(0, 0., 0., 0.);
173 setAxisBottom(0, 1., 0., 0.);
174 m_dVector.setValue(0, 0, 0);
175 m_eVector.setValue(0, 0, 0);
176 m_cachedRVector.setValue(0, 0, 0);
177 m_appliedForce.setValue(0, 0, 0);
178 m_appliedTorque.setValue(0, 0, 0);
179 m_appliedConstraintForce.setValue(0, 0, 0);
180 m_appliedConstraintTorque.setValue(0, 0, 0);
181 //
182 m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
183 m_jointPos[3] = 1.f; //"quat.w"
184 m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
185 m_cachedWorldTransform.setIdentity();
186 }
187
188 // routine to update m_cachedRotParentToThis and m_cachedRVector
189 void updateCacheMultiDof(btScalar *pq = 0)
190 {
191 btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
192
193 switch (m_jointType)
194 {
195 case eRevolute:
196 {
197 m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
198 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
199
200 break;
201 }
202 case ePrismatic:
203 {
204 // m_cachedRotParentToThis never changes, so no need to update
205 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0);
206
207 break;
208 }
209 case eSpherical:
210 {
211 m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
212 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
213
214 break;
215 }
216 case ePlanar:
217 {
218 m_cachedRotParentToThis = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis;
219 m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis, m_eVector);
220
221 break;
222 }
223 case eFixed:
224 {
225 m_cachedRotParentToThis = m_zeroRotParentToThis;
226 m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector);
227
228 break;
229 }
230 default:
231 {
232 //invalid type
233 btAssert(0);
234 }
235 }
236 }
237 };
238
239 #endif //BT_MULTIBODY_LINK_H
btMultibodyLink::m_jointLowerLimit
btScalar m_jointLowerLimit
Definition: btMultiBodyLink.h:144
btMultibodyLink::m_zeroRotParentToThis
btQuaternion m_zeroRotParentToThis
Definition: btMultiBodyLink.h:56
btMultiBodyLinkFlags
btMultiBodyLinkFlags
Definition: btMultiBodyLink.h:23
btMultibodyLink::m_appliedConstraintForce
btVector3 m_appliedConstraintForce
Definition: btMultiBodyLink.h:118
btMultibodyLink::m_jointFriction
btScalar m_jointFriction
Definition: btMultiBodyLink.h:143
btVector3::setValue
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:640
btMultibodyLink::getAxisTop
const btVector3 & getAxisTop(int dof) const
Definition: btMultiBodyLink.h:107
btVector3.h
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION
Definition: btMultiBodyLink.h:25
btMultibodyLink::eRevolute
Definition: btMultiBodyLink.h:72
btMultibodyLink::m_cachedRVector
btVector3 m_cachedRVector
Definition: btMultiBodyLink.h:113
btMultibodyLink::m_jointType
eFeatherstoneJointType m_jointType
Definition: btMultiBodyLink.h:132
btMultibodyLink::m_userPtr
const void * m_userPtr
Definition: btMultiBodyLink.h:140
btMultibodyLink::m_collider
class btMultiBodyLinkCollider * m_collider
Definition: btMultiBodyLink.h:127
btMultibodyLink::updateCacheMultiDof
void updateCacheMultiDof(btScalar *pq=0)
Definition: btMultiBodyLink.h:189
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION
Definition: btMultiBodyLink.h:26
btTransform::setIdentity
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:166
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMultibodyLink::setAxisTop
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
Definition: btMultiBodyLink.h:99
btMultibodyLink::m_posVarCount
int m_posVarCount
Definition: btMultiBodyLink.h:130
btMultibodyLink::m_absFrameLocVelocity
btSpatialMotionVector m_absFrameLocVelocity
Definition: btMultiBodyLink.h:68
btMultibodyLink::m_jointName
const char * m_jointName
Definition: btMultiBodyLink.h:139
btMultibodyLink::m_cfgOffset
int m_cfgOffset
Definition: btMultiBodyLink.h:110
btMultibodyLink
Definition: btMultiBodyLink.h:47
btMultiBodyJointFeedback
Definition: btMultiBodyJointFeedback.h:20
btMultibodyLink::btMultibodyLink
btMultibodyLink()
Definition: btMultiBodyLink.h:150
btMultibodyLink::m_dVector
btVector3 m_dVector
Definition: btMultiBodyLink.h:58
btMultibodyLink::getAxisBottom
const btVector3 & getAxisBottom(int dof) const
Definition: btMultiBodyLink.h:108
quatRotate
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:926
btMultibodyLink::eFixed
Definition: btMultiBodyLink.h:76
btSpatialAlgebra.h
btMultibodyLink::m_jointTorque
btScalar m_jointTorque[6]
Definition: btMultiBodyLink.h:125
btMultibodyLink::m_jointUpperLimit
btScalar m_jointUpperLimit
Definition: btMultiBodyLink.h:145
btQuaternion.h
btMultibodyLink::m_dofCount
int m_dofCount
Definition: btMultiBodyLink.h:130
btMultibodyLink::m_cachedRotParentToThis
btQuaternion m_cachedRotParentToThis
Definition: btMultiBodyLink.h:112
btMultibodyLink::eFeatherstoneJointType
eFeatherstoneJointType
Definition: btMultiBodyLink.h:70
btMultibodyLink::m_axes
btSpatialMotionVector m_axes[6]
Definition: btMultiBodyLink.h:93
btMultibodyLink::m_jointMaxForce
btScalar m_jointMaxForce
Definition: btMultiBodyLink.h:146
btMultibodyLink::m_linkName
const char * m_linkName
Definition: btMultiBodyLink.h:138
btMultibodyLink::m_mass
btScalar m_mass
Definition: btMultiBodyLink.h:51
btMultibodyLink::setAxisBottom
void setAxisBottom(int dof, const btVector3 &axis)
Definition: btMultiBodyLink.h:95
btSpatialMotionVector::m_bottomVec
btVector3 m_bottomVec
Definition: btSpatialAlgebra.h:96
btSpatialMotionVector
Definition: btSpatialAlgebra.h:94
btMultibodyLink::m_eVector
btVector3 m_eVector
Definition: btMultiBodyLink.h:66
btMultibodyLink::m_jointPos
btScalar m_jointPos[7]
Definition: btMultiBodyLink.h:121
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultibodyLink::m_dofOffset
int m_dofOffset
Definition: btMultiBodyLink.h:110
btMultibodyLink::m_jointDamping
btScalar m_jointDamping
Definition: btMultiBodyLink.h:142
btMultibodyLink::m_appliedTorque
btVector3 m_appliedTorque
Definition: btMultiBodyLink.h:116
btMultibodyLink::eSpherical
Definition: btMultiBodyLink.h:74
btMultibodyLink::eInvalid
Definition: btMultiBodyLink.h:77
btMultibodyLink::m_flags
int m_flags
Definition: btMultiBodyLink.h:128
btTransform
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:28
btMultibodyLink::m_absFrameTotVelocity
btSpatialMotionVector m_absFrameTotVelocity
Definition: btMultiBodyLink.h:68
btMultiBodyLinkCollider
Definition: btMultiBodyLinkCollider.h:32
btMultibodyLink::m_appliedConstraintTorque
btVector3 m_appliedConstraintTorque
Definition: btMultiBodyLink.h:119
btSpatialMotionVector::m_topVec
btVector3 m_topVec
Definition: btSpatialAlgebra.h:96
btMultibodyLink::BT_DECLARE_ALIGNED_ALLOCATOR
BT_DECLARE_ALIGNED_ALLOCATOR()
btCollisionObject.h
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:136
btMultibodyLink::setAxisTop
void setAxisTop(int dof, const btVector3 &axis)
Definition: btMultiBodyLink.h:94
btQuaternion
The btQuaternion implements quaternion to perform linear algebra rotations in combination with btMatr...
Definition: btQuaternion.h:49
btMultibodyLink::m_jointFeedback
struct btMultiBodyJointFeedback * m_jointFeedback
Definition: btMultiBodyLink.h:134
btMultibodyLink::m_inertiaLocal
btVector3 m_inertiaLocal
Definition: btMultiBodyLink.h:52
btMultibodyLink::m_appliedForce
btVector3 m_appliedForce
Definition: btMultiBodyLink.h:115
btMultibodyLink::setAxisBottom
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
Definition: btMultiBodyLink.h:103
btMultibodyLink::ePlanar
Definition: btMultiBodyLink.h:75
btMultibodyLink::ePrismatic
Definition: btMultiBodyLink.h:73
btMultibodyLink::m_parent
int m_parent
Definition: btMultiBodyLink.h:54
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMultibodyLink::m_jointMaxVelocity
btScalar m_jointMaxVelocity
Definition: btMultiBodyLink.h:147