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

add multibody interpolation transform so that collision detection is consistent with rigidbody

Browse Source
This commit is contained in:
Xuchen Han
2019-07-31 20:40:22 -07:00
parent ec403f790d
commit f1e7ce9ce1
10 changed files with 196 additions and 46 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -27,6 +27,7 @@
#include "btMultiBodyJointFeedback.h"
#include "LinearMath/btTransformUtil.h"
#include "LinearMath/btSerializer.h"
#include <iostream>
//#include "Bullet3Common/b3Logging.h"
// #define INCLUDE_GYRO_TERM
@@ -100,6 +101,8 @@ btMultiBody::btMultiBody(int n_links,
m_baseName(0),
m_basePos(0, 0, 0),
m_baseQuat(0, 0, 0, 1),
m_basePos_interpolate(0, 0, 0),
m_baseQuat_interpolate(0, 0, 0, 1),
m_baseMass(mass),
m_baseInertia(inertia),
@@ -449,6 +452,16 @@ const btQuaternion &btMultiBody::getParentToLocalRot(int i) const
return m_links[i].m_cachedRotParentToThis;
}
const btVector3 &btMultiBody::getInterpolateRVector(int i) const
{
return m_links[i].m_cachedRVector_interpolate;
}
const btQuaternion &btMultiBody::getInterpolateParentToLocalRot(int i) const
{
return m_links[i].m_cachedRotParentToThis_interpolate;
}
btVector3 btMultiBody::localPosToWorld(int i, const btVector3 &local_pos) const
{
btAssert(i >= -1);
@@ -1581,17 +1594,37 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
//printf("]\n");
/////////////////
}
void btMultiBody::predictPositionsMultiDof(btScalar dt)
{
stepPositionsMultiDof(dt, 0, 0, true);
}
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd)
void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd, bool predict)
{
int num_links = getNumLinks();
// step position by adding dt * velocity
//btVector3 v = getBaseVel();
//m_basePos += dt * v;
//
btScalar *pBasePos = (pq ? &pq[4] : m_basePos);
btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
//
btScalar *pBasePos;
btScalar *pBaseVel = (pqd ? &pqd[3] : &m_realBuf[3]); //note: the !pqd case assumes m_realBuf holds with base velocity at 3,4,5 (should be wrapped for safety)
if (!predict)
{
pBasePos = (pq ? &pq[4] : m_basePos);
} //
else
{
// reset to current position
for (int i = 0; i < 3; ++i)
{
m_basePos_interpolate[i] = m_basePos[i];
}
pBasePos = m_basePos_interpolate;
}
pBasePos[0] += dt * pBaseVel[0];
pBasePos[1] += dt * pBaseVel[1];
pBasePos[2] += dt * pBaseVel[2];
@@ -1645,7 +1678,18 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
//pQuatUpdateFun(getBaseOmega(), m_baseQuat, true, dt);
//
btScalar *pBaseQuat = pq ? pq : m_baseQuat;
btScalar *pBaseQuat;
if (!predict)
pBaseQuat = pq ? pq : m_baseQuat;
else
{
// reset to current orientation
for (int i = 0; i < 4; ++i)
{
m_baseQuat_interpolate[i] = m_baseQuat[i];
}
pBaseQuat = m_baseQuat_interpolate;
}
btScalar *pBaseOmega = pqd ? pqd : &m_realBuf[0]; //note: the !pqd case assumes m_realBuf starts with base omega (should be wrapped for safety)
//
btQuaternion baseQuat;
@@ -1670,7 +1714,12 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
// Finally we can update m_jointPos for each of the m_links
for (int i = 0; i < num_links; ++i)
{
btScalar *pJointPos = (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointPos;
if (!predict)
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
else
pJointPos = &m_links[i].m_jointPos_interpolate[0];
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
switch (m_links[i].m_jointType)
@@ -1678,12 +1727,23 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
//reset to current pos
if (predict)
{
pJointPos[0] = m_links[i].m_jointPos[0];
}
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
//reset to current pos
if (predict)
{
for (int i = 0; i < 4; ++i)
pJointPos[i] = m_links[i].m_jointPos[i];
}
btVector3 jointVel;
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri;
@@ -1697,6 +1757,11 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
case btMultibodyLink::ePlanar:
{
if (predict)
{
for (int i = 0; i < 3; ++i)
pJointPos[i] = m_links[i].m_jointPos[i];
}
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
@@ -1711,7 +1776,7 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
}
}
m_links[i].updateCacheMultiDof(pq);
m_links[i].updateCacheMultiDof(pq, predict);
if (pq)
pq += m_links[i].m_posVarCount;
@@ -2006,6 +2071,57 @@ void btMultiBody::updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQu
}
}
void btMultiBody::updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> &world_to_local, btAlignedObjectArray<btVector3> &local_origin)
{
world_to_local.resize(getNumLinks() + 1);
local_origin.resize(getNumLinks() + 1);
world_to_local[0] = getInterpolateWorldToBaseRot();
local_origin[0] = getInterpolateBasePos();
if (getBaseCollider())
{
btVector3 posr = local_origin[0];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
getBaseCollider()->setInterpolationWorldTransform(tr);
}
for (int k = 0; k < getNumLinks(); k++)
{
const int parent = getParent(k);
world_to_local[k + 1] = getInterpolateParentToLocalRot(k) * world_to_local[parent + 1];
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), getInterpolateRVector(k)));
}
for (int m = 0; m < getNumLinks(); m++)
{
btMultiBodyLinkCollider *col = getLink(m).m_collider;
if (col)
{
int link = col->m_link;
btAssert(link == m);
int index = link + 1;
btVector3 posr = local_origin[index];
// float pos[4]={posr.x(),posr.y(),posr.z(),1};
btScalar quat[4] = {-world_to_local[index].x(), -world_to_local[index].y(), -world_to_local[index].z(), world_to_local[index].w()};
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setInterpolationWorldTransform(tr);
}
}
}
int btMultiBody::calculateSerializeBufferSize() const
{
int sz = sizeof(btMultiBodyData);

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

@@ -193,12 +193,24 @@ public:
const btQuaternion &getWorldToBaseRot() const
{
return m_baseQuat;
} // rotates world vectors into base frame
}
const btVector3 &getInterpolateBasePos() const
{
return m_basePos_interpolate;
} // in world frame
const btQuaternion &getInterpolateWorldToBaseRot() const
{
return m_baseQuat_interpolate;
}
// rotates world vectors into base frame
btVector3 getBaseOmega() const { return btVector3(m_realBuf[0], m_realBuf[1], m_realBuf[2]); } // in world frame
void setBasePos(const btVector3 &pos)
{
m_basePos = pos;
m_basePos_interpolate = pos;
}
void setBaseWorldTransform(const btTransform &tr)
@@ -224,6 +236,7 @@ public:
void setWorldToBaseRot(const btQuaternion &rot)
{
m_baseQuat = rot; //m_baseQuat asumed to ba alias!?
m_baseQuat_interpolate = rot;
}
void setBaseOmega(const btVector3 &omega)
{
@@ -273,6 +286,8 @@ public:
const btVector3 &getRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
const btQuaternion &getParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
const btVector3 &getInterpolateRVector(int i) const; // vector from COM(parent(i)) to COM(i), in frame i's coords
const btQuaternion &getInterpolateParentToLocalRot(int i) const; // rotates vectors in frame parent(i) to vectors in frame i.
//
// transform vectors in local frame of link i to world frame (or vice versa)
@@ -420,7 +435,10 @@ public:
}
// timestep the positions (given current velocities).
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0, bool predict = false);
// predict the positions
void predictPositionsMultiDof(btScalar dt);
//
// contacts
@@ -581,6 +599,7 @@ public:
void compTreeLinkVelocities(btVector3 * omega, btVector3 * vel) const;
void updateCollisionObjectWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
void updateCollisionObjectInterpolationWorldTransforms(btAlignedObjectArray<btQuaternion> & world_to_local, btAlignedObjectArray<btVector3> & local_origin);
virtual int calculateSerializeBufferSize() const;
@@ -664,7 +683,9 @@ private:
const char *m_baseName; //memory needs to be manager by user!
btVector3 m_basePos; // position of COM of base (world frame)
btVector3 m_basePos_interpolate; // position of interpolated COM of base (world frame)
btQuaternion m_baseQuat; // rotates world points into base frame
btQuaternion m_baseQuat_interpolate;
btScalar m_baseMass; // mass of the base
btVector3 m_baseInertia; // inertia of the base (in local frame; diagonal)

22
src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp
View File

@@ -33,6 +33,12 @@ void btMultiBodyDynamicsWorld::removeMultiBody(btMultiBody* body)
m_multiBodies.remove(body);
}
void btMultiBodyDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
integrateMultiBodyTransforms(timeStep, /*predict = */ true);
}
void btMultiBodyDynamicsWorld::calculateSimulationIslands()
{
BT_PROFILE("calculateSimulationIslands");
@@ -778,8 +784,11 @@ void btMultiBodyDynamicsWorld::solveExternalForces(btContactSolverInfo& solverIn
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
{
btDiscreteDynamicsWorld::integrateTransforms(timeStep);
integrateMultiBodyTransforms(timeStep);
}
{
void btMultiBodyDynamicsWorld::integrateMultiBodyTransforms(btScalar timeStep, bool predict)
{
BT_PROFILE("btMultiBody stepPositions");
//integrate and update the Featherstone hierarchies
@@ -802,7 +811,7 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
int nLinks = bod->getNumLinks();
///base + num m_links
if (!predict)
{
if (!bod->isPosUpdated())
bod->stepPositionsMultiDof(timeStep);
@@ -815,18 +824,21 @@ void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
bod->setPosUpdated(false);
}
}
else
bod->predictPositionsMultiDof(timeStep);
m_scratch_world_to_local.resize(nLinks + 1);
m_scratch_local_origin.resize(nLinks + 1);
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
if (predict)
bod->updateCollisionObjectInterpolationWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
else
bod->updateCollisionObjectWorldTransforms(m_scratch_world_to_local, m_scratch_local_origin);
}
else
{
bod->clearVelocities();
}
}
}
}
void btMultiBodyDynamicsWorld::addMultiBodyConstraint(btMultiBodyConstraint* constraint)

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

@@ -96,7 +96,9 @@ public:
virtual void removeMultiBodyConstraint(btMultiBodyConstraint* constraint);
virtual void integrateTransforms(btScalar timeStep);
void integrateMultiBodyTransforms(btScalar timeStep,bool predict = false);
virtual void predictUnconstraintMotion(btScalar timeStep);
virtual void debugDrawWorld();
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);

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

@@ -112,6 +112,10 @@ struct btMultibodyLink
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
@@ -119,6 +123,7 @@ struct btMultibodyLink
btVector3 m_appliedConstraintTorque; // In WORLD frame
btScalar m_jointPos[7];
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
@@ -186,44 +191,50 @@ struct btMultibodyLink
}
// routine to update m_cachedRotParentToThis and m_cachedRVector
void updateCacheMultiDof(btScalar *pq = 0)
void updateCacheMultiDof(btScalar *pq = 0, bool predict = false)
{
btScalar *pJointPos = (pq ? pq : &m_jointPos[0]);
btScalar *pJointPos;
if (!predict)
pJointPos = (pq ? pq : &m_jointPos[0]);
else
pJointPos = &m_jointPos_interpolate[0];
btQuaternion& cachedRot = predict ? m_cachedRotParentToThis_interpolate : m_cachedRotParentToThis;
btVector3& cachedVector = predict ? m_cachedRVector_interpolate : 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;
}

5
src/BulletSoftBody/btDeformableBackwardEulerObjective.cpp
View File

@@ -12,12 +12,7 @@ btDeformableBackwardEulerObjective::btDeformableBackwardEulerObjective(btAligned
, projection(m_softBodies, m_dt, &m_indices)
, m_backupVelocity(backup_v)
{
// TODO: this should really be specified in initialization instead of here
// btDeformableMassSpringForce* mass_spring = new btDeformableMassSpringForce(m_softBodies);
// btDeformableGravityForce* gravity = new btDeformableGravityForce(m_softBodies, btVector3(0,-10,0));
m_preconditioner = new DefaultPreconditioner();
// m_lf.push_back(mass_spring);
// m_lf.push_back(gravity);
}
void btDeformableBackwardEulerObjective::reinitialize(bool nodeUpdated)

2
src/BulletSoftBody/btDeformableContactProjection.cpp
View File

@@ -44,7 +44,7 @@ static btVector3 generateUnitOrthogonalVector(const btVector3& u)
void btDeformableContactProjection::update()
{
///solve rigid body constraints
m_world->getSolverInfo().m_numIterations = 10;
m_world->getSolverInfo().m_numIterations = 1;
m_world->btMultiBodyDynamicsWorld::solveInternalConstraints(m_world->getSolverInfo());
// loop through constraints to set constrained values

2
src/BulletSoftBody/btDeformableRigidDynamicsWorld.cpp
View File

@@ -161,7 +161,7 @@ void btDeformableRigidDynamicsWorld::addSoftBody(btSoftBody* body, int collision
void btDeformableRigidDynamicsWorld::predictUnconstraintMotion(btScalar timeStep)
{
btDiscreteDynamicsWorld::predictUnconstraintMotion(timeStep);
btMultiBodyDynamicsWorld::predictUnconstraintMotion(timeStep);
m_deformableBodySolver->predictMotion(float(timeStep));
}

8
src/BulletSoftBody/btSoftBody.cpp
View File

@@ -2270,12 +2270,10 @@ bool btSoftBody::checkContact(const btCollisionObjectWrapper* colObjWrap,
{
btVector3 nrm;
const btCollisionShape* shp = colObjWrap->getCollisionShape();
const btRigidBody *tmpRigid = btRigidBody::upcast(colObjWrap->getCollisionObject());
const btCollisionObject* tmpCollisionObj = colObjWrap->getCollisionObject();
// get the position x_{n+1}^* = x_n + dt * v_{n+1}^* where v_{n+1}^* = v_n + dtg
const btTransform &wtr = (tmpRigid&&predict) ? tmpRigid->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
// const btTransform &wtr = predict ? colObjWrap->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
// TODO: get the correct transform for multibody
const btTransform &wtr = (predict) ? tmpCollisionObj->getInterpolationWorldTransform() : colObjWrap->getWorldTransform();
// const btTransform &wtr = colObjWrap->getWorldTransform();
btScalar dst =
m_worldInfo->m_sparsesdf.Evaluate(

9
src/BulletSoftBody/btSoftBodyInternals.h
View File

@@ -985,13 +985,8 @@ struct btSoftColliders
btVector3 t2 = btCross(normal, t1);
btMultiBodyJacobianData jacobianData_normal, jacobianData_t1, jacobianData_t2;
findJacobian(multibodyLinkCol, jacobianData_normal, c.m_node->m_q, normal);
// findJacobian is hella expensive, avoid calling if possible
if (fc != 0)
{
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
}
findJacobian(multibodyLinkCol, jacobianData_t1, c.m_node->m_q, t1);
findJacobian(multibodyLinkCol, jacobianData_t2, c.m_node->m_q, t2);
btScalar* J_n = &jacobianData_normal.m_jacobians[0];
btScalar* J_t1 = &jacobianData_t1.m_jacobians[0];