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

add 'fixed' joint for btMultiBody

Browse Source 
improve btMultiBody version of URDF reader (still work-in-progress)
enabled planar joint for btMultiBody (untested)
enable loading from relative path for .stl meshes
This commit is contained in:
erwin coumans
2014-08-28 18:42:08 -07:00
parent 3c558ec995
commit 89addd438e
19 changed files with 1276 additions and 304 deletions
Download Patch File Download Diff File Expand all files Collapse all files

94
src/BulletDynamics/Featherstone/btMultiBody.cpp
View File

@@ -132,6 +132,38 @@ btMultiBody::~btMultiBody()
{
}
void btMultiBody::setupFixed(int i,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
bool disableParentCollision)
{
btAssert(m_isMultiDof);
m_links[i].m_mass = mass;
m_links[i].m_inertia = inertia;
m_links[i].m_parent = parent;
m_links[i].m_zeroRotParentToThis = rotParentToThis;
m_links[i].m_eVector = parentComToThisComOffset;
m_links[i].m_jointType = btMultibodyLink::eFixed;
m_links[i].m_dofCount = 0;
m_links[i].m_posVarCount = 0;
if (disableParentCollision)
m_links[i].m_flags |=BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION;
//
m_links[i].updateCacheMultiDof();
//
//if(m_isMultiDof)
// resizeInternalMultiDofBuffers();
//
updateLinksDofOffsets();
}
void btMultiBody::setupPrismatic(int i,
btScalar mass,
const btVector3 &inertia,
@@ -290,12 +322,15 @@ void btMultiBody::setupPlanar(int i,
m_links[i].m_jointType = btMultibodyLink::ePlanar;
m_links[i].m_dofCount = 3;
m_links[i].m_posVarCount = 3;
m_links[i].getAxisTop(0) = rotationAxis.normalized();
m_links[i].getAxisTop(1).setZero();
m_links[i].getAxisTop(2).setZero();
m_links[i].getAxisBottom(0).setZero();
m_links[i].getAxisBottom(1) = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
m_links[i].getAxisBottom(2) = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
btVector3 n=rotationAxis.normalized();
m_links[i].setAxisTop(0, n[0],n[1],n[2]);
m_links[i].setAxisTop(1,0,0,0);
m_links[i].setAxisTop(2,0,0,0);
m_links[i].setAxisBottom(0,0,0,0);
btVector3 cr = m_links[i].getAxisTop(0).cross(vecNonParallelToRotAxis);
m_links[i].setAxisBottom(1,cr[0],cr[1],cr[2]);
cr = m_links[i].getAxisBottom(1).cross(m_links[i].getAxisTop(0));
m_links[i].setAxisBottom(2,cr[0],cr[1],cr[2]);
m_links[i].m_jointPos[0] = m_links[i].m_jointPos[1] = m_links[i].m_jointPos[2] = 0.f;
m_links[i].m_jointTorque[0] = m_links[i].m_jointTorque[1] = m_links[i].m_jointTorque[2] = 0.f;
@@ -1171,41 +1206,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
fromParent.m_rotMat = rot_from_parent[i+1]; fromParent.m_trnVec = m_links[i].m_cachedRVector;
fromWorld.m_rotMat = rot_from_world[i+1];
////clamp parent's omega
//btScalar parOmegaMod = spatVel[parent+1].getAngular().length();
//btScalar parOmegaModMax = 0.1;
//if(parOmegaMod > parOmegaModMax)
//{
// //btSpatialMotionVector clampedParVel(spatVel[parent+1].getAngular() * parOmegaModMax / parOmegaMod, spatVel[parent+1].getLinear());
// btSpatialMotionVector clampedParVel; clampedParVel = spatVel[parent+1] * (parOmegaModMax / parOmegaMod);
// fromParent.transform(clampedParVel, spatVel[i+1]);
// spatVel[parent+1] *= (parOmegaModMax / parOmegaMod);
//}
//else
{
// vhat_i = i_xhat_p(i) * vhat_p(i)
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
//nice alternative below (using operator *) but it generates temps
}
//////////////////////////////////////////////////////////////
//if(m_links[i].m_jointType == btMultibodyLink::eRevolute || m_links[i].m_jointType == btMultibodyLink::eSpherical)
//{
// btScalar mod2 = 0;
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// mod2 += getJointVelMultiDof(i)[dof]*getJointVelMultiDof(i)[dof];
// btScalar angvel = sqrt(mod2);
// btScalar maxAngVel = 6;//SIMD_HALF_PI * 0.075;
// btScalar step = 1; //dt
// if (angvel*step > maxAngVel)
// {
// btScalar * qd = getJointVelMultiDof(i);
// for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
// qd[dof] *= (maxAngVel/step) /angvel;
// }
//}
fromParent.transform(spatVel[parent+1], spatVel[i+1]);
// now set vhat_i to its true value by doing
// vhat_i += qidot * shat_i
@@ -1717,13 +1718,14 @@ void btMultiBody::stepVelocities(btScalar dt,
h_bottom[i] = inertia_bottom_left[i+1] * m_links[i].getAxisTop(0) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(0);
btScalar val = SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), h_top[i], h_bottom[i]);
D[i] = val;
Y[i] = m_links[i].m_jointTorque[0]
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), zero_acc_top_angular[i+1], zero_acc_bottom_linear[i+1])
- SpatialDotProduct(h_top[i], h_bottom[i], coriolis_top_angular[i], coriolis_bottom_linear[i]);
const int parent = m_links[i].m_parent;
btAssert(D[i]!=0.f);
// Ip += pXi * (Ii - hi hi' / Di) * iXp
const btScalar one_over_di = 1.0f / D[i];
@@ -2637,7 +2639,7 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
btAssert(v_ptr - &scratch_v[0] == scratch_v.size());
scratch_r.resize(m_dofCount);
btScalar * results = num_links > 0 ? &scratch_r[0] : 0;
btScalar * results = m_dofCount > 0 ? &scratch_r[0] : 0;
btMatrix3x3 * rot_from_world = &scratch_m[0];
@@ -2717,9 +2719,9 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
case btMultibodyLink::ePlanar:
{
results[m_links[i].m_dofOffset + 0] = n_local[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local[i+1].dot(m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local[i+1].dot(m_links[i].getAxisBottom(2));
results[m_links[i].m_dofOffset + 0] = n_local_lin[i+1].dot(m_links[i].getAxisTop(0).cross(p_minus_com_local[i+1]));// + m_links[i].getAxisBottom(0));
results[m_links[i].m_dofOffset + 1] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(1));
results[m_links[i].m_dofOffset + 2] = n_local_lin[i+1].dot(m_links[i].getAxisBottom(2));
break;
}

17
src/BulletDynamics/Featherstone/btMultiBody.h
View File

@@ -56,7 +56,16 @@ public:
~btMultiBody();
void setupPrismatic(int i, // 0 to num_links-1
void btMultiBody::setupFixed(int linkIndex,
btScalar mass,
const btVector3 &inertia,
int parent,
const btQuaternion &rotParentToThis,
const btVector3 &parentComToThisComOffset,
bool disableParentCollision);
void setupPrismatic(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia, // in my frame; assumed diagonal
int parent,
@@ -66,17 +75,17 @@ public:
bool disableParentCollision=false
);
void setupRevolute(int i, // 0 to num_links-1
void setupRevolute(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,
int parentIndex,
const btQuaternion &rotParentToThis, // rotate points in parent frame to this frame, when q = 0
const btVector3 &jointAxis, // in my frame
const btVector3 &parentComToThisPivotOffset, // vector from parent COM to joint axis, in PARENT frame
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
bool disableParentCollision=false);
void setupSpherical(int i, // 0 to num_links-1
void setupSpherical(int linkIndex, // 0 to num_links-1
btScalar mass,
const btVector3 &inertia,
int parent,

4
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h
View File

@@ -38,7 +38,7 @@ protected:
virtual void calculateSimulationIslands();
virtual void updateActivationState(btScalar timeStep);
virtual void solveConstraints(btContactSolverInfo& solverInfo);
virtual void integrateTransforms(btScalar timeStep);
public:
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
@@ -52,5 +52,7 @@ public:
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void removeMultiBodyConstraint( btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

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

@@ -25,7 +25,7 @@ enum btMultiBodyLinkFlags
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
};
//#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
#define TEST_SPATIAL_ALGEBRA_LAYER
//
@@ -368,7 +368,9 @@ 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
{
@@ -378,6 +380,7 @@ struct btMultibodyLink
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
ePlanar = 3,
#endif
eFixed = 4,
eInvalid
};
@@ -505,11 +508,18 @@ struct btMultibodyLink
case ePlanar:
{
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * m_axesBottom[1] + pJointPos[2] * m_axesBottom[2]) + quatRotate(m_cachedRotParentToThis,m_eVector);
m_cachedRVector = quatRotate(btQuaternion(getAxisTop(0),-pJointPos[0]), pJointPos[1] * getAxisBottom(1) + pJointPos[2] * getAxisBottom(2)) + quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
#endif
case eFixed:
{
m_cachedRotParentToThis = m_zeroRotParentToThis;
m_cachedRVector = quatRotate(m_cachedRotParentToThis,m_eVector);
break;
}
default:
{
//invalid type