Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

First step in btMultiBody joint force/torque feedback. There is still some work to be done for 'mobilizer limit/motor'.

Browse Source 
See examples/MultiBody/TestJointTorqueSetup and examples/Constraints/TestHingeTorque for joint feedback.
This commit is contained in:
erwincoumans
2015-06-19 09:18:27 -07:00
parent 41aa58560b
commit 89edc40d61
16 changed files with 689 additions and 768 deletions
Download Patch File Download Diff File Expand all files Collapse all files

29
src/BulletDynamics/Featherstone/btMultiBodyLink.h
View File

@@ -25,6 +25,7 @@ enum btMultiBodyLinkFlags
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//both defines are now permanently enabled
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
@@ -34,11 +35,9 @@ enum btMultiBodyLinkFlags
//namespace {
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
#include "LinearMath/btSpatialAlgebra.h"
#endif
//}
//
@@ -64,18 +63,14 @@ struct btMultibodyLink
// revolute: vector from parent's COM to the pivot point, in PARENT's frame.
btVector3 m_eVector;
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
btSpatialMotionVector m_absFrameTotVelocity, m_absFrameLocVelocity;
#endif
enum eFeatherstoneJointType
{
eRevolute = 0,
ePrismatic = 1,
eSpherical = 2,
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
ePlanar = 3,
#endif
eFixed = 4,
eInvalid
};
@@ -95,16 +90,6 @@ struct btMultibodyLink
// m_axesTop[1][2] = zero
// m_axesBottom[0] = zero
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
btVector3 m_axesTop[6];
btVector3 m_axesBottom[6];
void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; }
void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; }
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); }
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); }
const btVector3 & getAxisTop(int dof) const { return m_axesTop[dof]; }
const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; }
#else
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; }
@@ -112,7 +97,6 @@ struct btMultibodyLink
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; }
#endif
int m_dofOffset, m_cfgOffset;
@@ -133,7 +117,12 @@ struct btMultibodyLink
int m_dofCount, m_posVarCount; //redundant but handy
eFeatherstoneJointType m_jointType;
struct btMultiBodyJointFeedback* m_jointFeedback;
btVector3 m_worldPosition;
// ctor: set some sensible defaults
btMultibodyLink()
@@ -145,7 +134,9 @@ struct btMultibodyLink
m_flags(0),
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid)
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_worldPosition(0,0,0)
{
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
@@ -203,7 +194,6 @@ struct btMultibodyLink
break;
}
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
@@ -211,7 +201,6 @@ struct btMultibodyLink
break;
}
#endif
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;