diff options
Diffstat (limited to 'extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h')
| -rw-r--r-- | extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h | 241 |
1 files changed, 161 insertions, 80 deletions
diff --git a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 668e4443904..01d5583c2fd 100644 --- a/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/extern/bullet2/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -20,9 +20,10 @@ subject to the following restrictions: #include "LinearMath/btVector3.h" #include "BulletCollision/CollisionDispatch/btCollisionObject.h" -enum btMultiBodyLinkFlags +enum btMultiBodyLinkFlags { - BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1 + BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1, + BT_MULTIBODYLINKFLAGS_DISABLE_ALL_PARENT_COLLISION = 2, }; //both defines are now permanently enabled @@ -35,7 +36,6 @@ enum btMultiBodyLinkFlags //namespace { - #include "LinearMath/btSpatialAlgebra.h" //} @@ -44,27 +44,26 @@ enum btMultiBodyLinkFlags // Link struct // -struct btMultibodyLink +struct btMultibodyLink { - BT_DECLARE_ALIGNED_ALLOCATOR(); - btScalar m_mass; // mass of link - btVector3 m_inertiaLocal; // inertia of link (local frame; diagonal) + 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. - 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. - 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) - 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. + // 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; + + btVector3 m_eVector; btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity; @@ -78,13 +77,11 @@ struct btMultibodyLink 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 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) @@ -92,117 +89,149 @@ struct btMultibodyLink // 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 + // 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; } + 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. + 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. + + // predicted verstion + btQuaternion m_cachedRotParentToThis_interpolate; // rotates vectors in parent frame to vectors in local frame + btVector3 m_cachedRVector_interpolate; // 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_appliedForce; // In WORLD frame + btVector3 m_appliedTorque; // In WORLD frame -btVector3 m_appliedConstraintForce; // In WORLD frame - btVector3 m_appliedConstraintTorque; // 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_jointPos_interpolate[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; + + class btMultiBodyLinkCollider *m_collider; int m_flags; - - - int m_dofCount, m_posVarCount; //redundant but handy - + + 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 + struct btMultiBodyJointFeedback *m_jointFeedback; - 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! + btTransform m_cachedWorldTransform; //this cache is updated when calling btMultiBody::forwardKinematics - // ctor: set some sensible defaults + 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_mass(1), + m_parent(-1), + m_zeroRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis(0, 0, 0, 1), + m_cachedRotParentToThis_interpolate(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_cachedRVector_interpolate.setValue(0, 0, 0); + m_appliedForce.setValue(0, 0, 0); m_appliedTorque.setValue(0, 0, 0); - // + m_appliedConstraintForce.setValue(0, 0, 0); + m_appliedConstraintTorque.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_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 + // routine to update m_cachedRotParentToThis and m_cachedRVector void updateCacheMultiDof(btScalar *pq = 0) { - btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); - - switch(m_jointType) + btScalar *pJointPos = (pq ? pq : &m_jointPos[0]); + btQuaternion& cachedRot = m_cachedRotParentToThis; + btVector3& cachedVector = m_cachedRVector; + switch (m_jointType) { case eRevolute: { - m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = 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); + cachedVector = 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); + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, 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); + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); break; } case eFixed: { - m_cachedRotParentToThis = m_zeroRotParentToThis; - m_cachedRVector = m_dVector + quatRotate(m_cachedRotParentToThis,m_eVector); + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); break; } @@ -212,8 +241,60 @@ btVector3 m_appliedConstraintForce; // In WORLD frame btAssert(0); } } + m_cachedRotParentToThis_interpolate = m_cachedRotParentToThis; + m_cachedRVector_interpolate = m_cachedRVector; } + + void updateInterpolationCacheMultiDof() + { + btScalar *pJointPos = &m_jointPos_interpolate[0]; + + btQuaternion& cachedRot = m_cachedRotParentToThis_interpolate; + btVector3& cachedVector = m_cachedRVector_interpolate; + switch (m_jointType) + { + case eRevolute: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector); + + break; + } + case ePrismatic: + { + // m_cachedRotParentToThis never changes, so no need to update + cachedVector = m_dVector + quatRotate(m_cachedRotParentToThis, m_eVector) + pJointPos[0] * getAxisBottom(0); + + break; + } + case eSpherical: + { + cachedRot = btQuaternion(pJointPos[0], pJointPos[1], pJointPos[2], -pJointPos[3]) * m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + case ePlanar: + { + cachedRot = btQuaternion(getAxisTop(0), -pJointPos[0]) * m_zeroRotParentToThis; + cachedVector = quatRotate(btQuaternion(getAxisTop(0), -pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(cachedRot, m_eVector); + + break; + } + case eFixed: + { + cachedRot = m_zeroRotParentToThis; + cachedVector = m_dVector + quatRotate(cachedRot, m_eVector); + + break; + } + default: + { + //invalid type + btAssert(0); + } + } + } }; - -#endif //BT_MULTIBODY_LINK_H +#endif //BT_MULTIBODY_LINK_H |