1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
|
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
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.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#ifndef BT_MULTIBODY_LINK_H
#define BT_MULTIBODY_LINK_H
#include "LinearMath/btQuaternion.h"
#include "LinearMath/btVector3.h"
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
enum btMultiBodyLinkFlags
{
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1,
BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2,
};
//both defines are now permanently enabled
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
// Various spatial helper functions
//
//namespace {
#include "LinearMath/btSpatialAlgebra.h"
//}
//
// Link struct
//
struct btMultibodyLink
{
BT_DECLARE_ALIGNED_ALLOCATOR();
btScalar m_mass; // mass of link
btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal)
int m_parent; // index of the parent link (assumed to be < index of this link), or -1 if parent is the base link.
btQuaternion m_zeroRotParentToThis; // rotates vectors in parent-frame to vectors in local-frame (when q=0). constant.
btVector3 m_dVector; // vector from the inboard joint pos to this link's COM. (local frame.) constant.
//this is set to zero for planar joint (see also m_eVector comment)
// m_eVector is constant, but depends on the joint type:
// revolute, fixed, prismatic, spherical: vector from parent's COM to the pivot point, in PARENT's frame.
// planar: vector from COM of parent to COM of this link, WHEN Q = 0. (local frame.)
// todo: fix the planar so it is consistent with the other joints
btVector3 m_eVector;
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
enum eFeatherstoneJointType
{
eRevolute = 0,
ePrismatic = 1,
eSpherical = 2,
ePlanar = 3,
eFixed = 4,
eInvalid
};
// "axis" = spatial joint axis (Mirtich Defn 9 p104). (expressed in local frame.) constant.
// for prismatic: m_axesTop[0] = zero;
// m_axesBottom[0] = unit vector along the joint axis.
// for revolute: m_axesTop[0] = unit vector along the rotation axis (u);
// m_axesBottom[0] = u cross m_dVector (i.e. COM linear motion due to the rotation at the joint)
//
// for spherical: m_axesTop[0][1][2] (u1,u2,u3) form a 3x3 identity matrix (3 rotation axes)
// m_axesBottom[0][1][2] cross u1,u2,u3 (i.e. COM linear motion due to the rotation at the joint)
//
// for planar: m_axesTop[0] = unit vector along the rotation axis (u); defines the plane of motion
// m_axesTop[1][2] = zero
// m_axesBottom[0] = zero
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
btSpatialMotionVector m_axes[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
void setAxisBottom(int dof, const btVector3 &axis)
{
m_axes[dof].m_bottomVec = axis;
}
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_topVec.setValue(x, y, z);
}
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z)
{
m_axes[dof].m_bottomVec.setValue(x, y, z);
}
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
int m_dofOffset, m_cfgOffset;
btQuaternion m_cachedRotParentToThis; // rotates vectors in parent frame to vectors in local frame
btVector3 m_cachedRVector; // vector from COM of parent to COM of this link, in local frame.
btVector3 m_appliedForce; // In WORLD frame
btVector3 m_appliedTorque; // In WORLD frame
btVector3 m_appliedConstraintForce; // In WORLD frame
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
//It gets set to zero after each internal stepSimulation call
btScalar m_jointTorque[6];
class btMultiBodyLinkCollider* m_collider;
int m_flags;
int m_dofCount, m_posVarCount; //redundant but handy
eFeatherstoneJointType m_jointType;
struct btMultiBodyJointFeedback* m_jointFeedback;
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
const char* m_linkName;//m_linkName memory needs to be managed by the developer/user!
const char* m_jointName;//m_jointName memory needs to be managed by the developer/user!
const void* m_userPtr;//m_userPtr ptr needs to be managed by the developer/user!
btScalar m_jointDamping; //todo: implement this internally. It is unused for now, it is set by a URDF loader. User can apply manual damping.
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.
btScalar m_jointLowerLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointUpperLimit; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxForce; //todo: implement this internally. It is unused for now, it is set by a URDF loader.
btScalar m_jointMaxVelocity;//todo: implement this internally. It is unused for now, it is set by a URDF loader.
// ctor: set some sensible defaults
btMultibodyLink()
: m_mass(1),
m_parent(-1),
m_zeroRotParentToThis(0, 0, 0, 1),
m_cachedRotParentToThis(0, 0, 0, 1),
m_collider(0),
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_linkName(0),
m_jointName(0),
m_userPtr(0),
m_jointDamping(0),
m_jointFriction(0),
m_jointLowerLimit(0),
m_jointUpperLimit(0),
m_jointMaxForce(0),
m_jointMaxVelocity(0)
{
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
m_dVector.setValue(0, 0, 0);
m_eVector.setValue(0, 0, 0);
m_cachedRVector.setValue(0, 0, 0);
m_appliedForce.setValue( 0, 0, 0);
m_appliedTorque.setValue(0, 0, 0);
//
m_jointPos[0] = m_jointPos[1] = m_jointPos[2] = m_jointPos[4] = m_jointPos[5] = m_jointPos[6] = 0.f;
m_jointPos[3] = 1.f; //"quat.w"
m_jointTorque[0] = m_jointTorque[1] = m_jointTorque[2] = m_jointTorque[3] = m_jointTorque[4] = m_jointTorque[5] = 0.f;
m_cachedWorldTransform.setIdentity();
}
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
switch(m_jointType)
{
case eRevolute:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePrismatic:
{
// m_cachedRotParentToThis never changes, so no need to update
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector) + pJointPos[0] * getAxisBottom(0);
break;
}
case eSpherical:
{
m_cachedRotParentToThis = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
default:
{
//invalid type
btAssert(0);
}
}
}
};
#endif //BT_MULTIBODY_LINK_H
|