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

implement joint reaction forces for mobilizer motor/limit, by passing the constraint forces as 'external forces' and going through the Articulated Body Algorithm

Browse Source 
minor refactor for forwardKinematics, store the cached world transform in each btMultiBody::link
This commit is contained in:
erwin coumans
2015-06-19 15:51:24 -07:00
parent 89edc40d61
commit 6e9eb13235
9 changed files with 178 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1553,6 +1553,7 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
// Temporary matrices/vectors -- use scratch space from caller
// so that we don't have to keep reallocating every frame
int num_links = getNumLinks();
scratch_r.resize(m_dofCount);
scratch_v.resize(4*num_links + 4);
@@ -2295,4 +2296,50 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
}
void btMultiBody::forwardKinematics(btAlignedObjectArray<btQuaternion>& world_to_local,btAlignedObjectArray<btVector3>& local_origin)
{
int num_links = getNumLinks();
// Cached 3x3 rotation matrices from parent frame to this frame.
btMatrix3x3* rot_from_parent =(btMatrix3x3 *) &m_matrixBuf[0];
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
for (int i = 0; i < num_links; ++i)
{
const int parent = m_links[i].m_parent;
rot_from_parent[i+1] = btMatrix3x3(m_links[i].m_cachedRotParentToThis);
}
int nLinks = getNumLinks();
///base + num m_links
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
world_to_local[0] = getWorldToBaseRot();
local_origin[0] = getBasePos();
for (int k=0;k<getNumLinks();k++)
{
const int parent = getParent(k);
world_to_local[k+1] = getParentToLocalRot(k) * world_to_local[parent+1];
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , getRVector(k)));
}
for (int link=0;link<getNumLinks();link++)
{
int index = link+1;
btVector3 posr = local_origin[index];
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]));
getLink(link).m_cachedWorldTransform = tr;
}
}

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

@@ -190,6 +190,7 @@ public:
btTransform tr;
tr.setOrigin(getBasePos());
tr.setRotation(getWorldToBaseRot().inverse());
return tr;
}
void setBaseVel(const btVector3 &vel)
@@ -552,6 +553,8 @@ public:
{
return m_internalNeedsJointFeedback;
}
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
private:
btMultiBody(const btMultiBody &); // not implemented

4
src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp
View File

@@ -98,7 +98,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
rel_pos1 = posAworld - multiBodyA->getBasePos();
} else
{
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_worldPosition;
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
@@ -164,7 +164,7 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
rel_pos2 = posBworld - multiBodyB->getBasePos();
} else
{
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_worldPosition;
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;

6
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -294,7 +294,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
rel_pos1 = pos1 - multiBodyA->getBasePos();
} else
{
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_worldPosition;
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
}
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
@@ -346,7 +346,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
rel_pos2 = pos2 - multiBodyB->getBasePos();
} else
{
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_worldPosition;
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
}
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
@@ -957,7 +957,7 @@ void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolv
if (!c.m_useJointForce)
{
c.m_multiBodyB->addLinkForce(c.m_linkB,force);
b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
//b3Printf("t = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
c.m_multiBodyB->addLinkTorque(c.m_linkB,torque);
}

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

@@ -399,40 +399,10 @@ void btMultiBodyDynamicsWorld::forwardKinematics()
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
for (int b=0;b<m_multiBodies.size();b++)
{
btMultiBody* bod = m_multiBodies[b];
int nLinks = bod->getNumLinks();
///base + num m_links
world_to_local.resize(nLinks+1);
local_origin.resize(nLinks+1);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
for (int k=0;k<bod->getNumLinks();k++)
{
const int parent = bod->getParent(k);
world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
}
for (int link=0;link<bod->getNumLinks();link++)
{
int index = link+1;
btVector3 posr = local_origin[index];
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]));
bod->getLink(link).m_worldPosition = posr;
}
bod->forwardKinematics(world_to_local,local_origin);
}
}
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
@@ -507,10 +477,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}
}
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
{
BT_PROFILE("btMultiBody stepVelocities");
@@ -709,6 +676,11 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
}
}
m_solverMultiBodyIslandCallback->processConstraints();
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
}
@@ -850,8 +822,8 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
{
BT_PROFILE("btMultiBody debugDrawWorld");
btAlignedObjectArray<btQuaternion> world_to_local;
btAlignedObjectArray<btVector3> local_origin;
btAlignedObjectArray<btQuaternion> world_to_local1;
btAlignedObjectArray<btVector3> local_origin1;
for (int c=0;c<m_multiBodyConstraints.size();c++)
{
@@ -862,52 +834,40 @@ void btMultiBodyDynamicsWorld::debugDrawWorld()
for (int b = 0; b<m_multiBodies.size(); b++)
{
btMultiBody* bod = m_multiBodies[b];
int nLinks = bod->getNumLinks();
///base + num m_links
world_to_local.resize(nLinks + 1);
local_origin.resize(nLinks + 1);
world_to_local[0] = bod->getWorldToBaseRot();
local_origin[0] = bod->getBasePos();
bod->forwardKinematics(world_to_local1,local_origin1);
{
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]));
getDebugDrawer()->drawTransform(tr, 0.1);
}
for (int k = 0; k<bod->getNumLinks(); k++)
{
const int parent = bod->getParent(k);
world_to_local[k + 1] = bod->getParentToLocalRot(k) * world_to_local[parent + 1];
local_origin[k + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[k + 1].inverse(), bod->getRVector(k)));
}
getDebugDrawer()->drawTransform(bod->getBaseWorldTransform(), 0.1);
int nLinks = bod->getNumLinks();
for (int m = 0; m<bod->getNumLinks(); m++)
{
int 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]));
const btTransform& tr = bod->getLink(m).m_cachedWorldTransform;
getDebugDrawer()->drawTransform(tr, 0.1);
//draw the joint axis
if (bod->getLink(m).m_jointType==btMultibodyLink::eRevolute)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_topVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
if (bod->getLink(m).m_jointType==btMultibodyLink::ePrismatic)
{
btVector3 vec = quatRotate(tr.getRotation(),bod->getLink(m).m_axes[0].m_bottomVec);
btVector4 color(0,0,0,1);//1,1,1);
btVector3 from = vec+tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
btVector3 to = tr.getOrigin()-quatRotate(tr.getRotation(),bod->getLink(m).m_dVector);
getDebugDrawer()->drawLine(from,to,color);
}
}
}
}

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

@@ -85,5 +85,6 @@ public:
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
void forwardKinematics();
};
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H

47
src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp
View File

@@ -21,9 +21,10 @@ subject to the following restrictions:
#include "BulletCollision/CollisionDispatch/btCollisionObject.h"
btMultiBodyJointLimitConstraint::btMultiBodyJointLimitConstraint(btMultiBody* body, int link, btScalar lower, btScalar upper)
//:btMultiBodyConstraint(body,0,link,-1,2,true),
:btMultiBodyConstraint(body,body,link,link,2,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,2,true),
m_lowerBound(lower),
m_upperBound(upper)
{
@@ -43,7 +44,6 @@ void btMultiBodyJointLimitConstraint::finalizeMultiDof()
jacobianA(0)[offset] = 1;
// row 1: the upper bound
//jacobianA(1)[offset] = -1;
jacobianB(1)[offset] = -1;
m_numDofsFinalized = m_jacSizeBoth;
@@ -92,6 +92,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
btMultiBodyJacobianData& data,
const btContactSolverInfo& infoGlobal)
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
@@ -106,9 +107,12 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
// row 1: the upper bound
setPosition(1, m_upperBound - m_bodyA->getJointPos(m_linkA));
for (int row=0;row<getNumRows();row++)
{
btScalar direction = row? -1 : 1;
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
constraintRow.m_multiBodyA = m_bodyA;
constraintRow.m_multiBodyB = m_bodyB;
@@ -116,7 +120,42 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
const btVector3 dummy(0, 0, 0);
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
constraintRow.m_useJointForce = true;
if (m_bodyA->isMultiDof())
{
constraintRow.m_useJointForce = false;
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = direction*quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = direction* quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
{
btScalar penetration = getPosition(row);
btScalar positionalError = 0.f;

45
src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp
View File

@@ -22,11 +22,11 @@ subject to the following restrictions:
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse)
:btMultiBodyConstraint(body,body,link,link,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
{
int parent = body->getLink(link).m_parent;
m_maxAppliedImpulse = maxMotorImpulse;
// the data.m_jacobians never change, so may as well
// initialize them here
@@ -51,7 +51,7 @@ void btMultiBodyJointMotor::finalizeMultiDof()
btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse)
//:btMultiBodyConstraint(body,0,link,-1,1,true),
:btMultiBodyConstraint(body,body,link,link,1,true),
:btMultiBodyConstraint(body,body,link,body->getLink(link).m_parent,1,true),
m_desiredVelocity(desiredVelocity)
{
btAssert(linkDoF < body->getLink(link).m_dofCount);
@@ -98,7 +98,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
{
// only positions need to be updated -- data.m_jacobians and force
// directions were set in the ctor and never change.
if (m_numDofsFinalized != m_jacSizeBoth)
{
finalizeMultiDof();
@@ -117,7 +117,42 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
constraintRow.m_useJointForce = true;
if (m_bodyA->isMultiDof())
{
constraintRow.m_useJointForce = false;
//expect either prismatic or revolute joint type for now
btAssert((m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::eRevolute)||(m_bodyA->getLink(m_linkA).m_jointType == btMultibodyLink::ePrismatic));
switch (m_bodyA->getLink(m_linkA).m_jointType)
{
case btMultibodyLink::eRevolute:
{
constraintRow.m_contactNormal1.setZero();
constraintRow.m_contactNormal2.setZero();
btVector3 revoluteAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_topVec);
constraintRow.m_relpos1CrossNormal=revoluteAxisInWorld;
constraintRow.m_relpos2CrossNormal=-revoluteAxisInWorld;
break;
}
case btMultibodyLink::ePrismatic:
{
btVector3 prismaticAxisInWorld = quatRotate(m_bodyA->getLink(m_linkA).m_cachedWorldTransform.getRotation(),m_bodyA->getLink(m_linkA).m_axes[0].m_bottomVec);
constraintRow.m_contactNormal1=prismaticAxisInWorld;
constraintRow.m_contactNormal2=-prismaticAxisInWorld;
constraintRow.m_relpos1CrossNormal.setZero();
constraintRow.m_relpos2CrossNormal.setZero();
break;
}
default:
{
btAssert(0);
}
};
}
}
}

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

@@ -122,7 +122,7 @@ struct btMultibodyLink
struct btMultiBodyJointFeedback* m_jointFeedback;
btVector3 m_worldPosition;
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
// ctor: set some sensible defaults
btMultibodyLink()
@@ -135,9 +135,10 @@ struct btMultibodyLink
m_dofCount(0),
m_posVarCount(0),
m_jointType(btMultibodyLink::eInvalid),
m_jointFeedback(0),
m_worldPosition(0,0,0)
m_jointFeedback(0)
{
m_inertiaLocal.setValue(1, 1, 1);
setAxisTop(0, 0., 0., 0.);
setAxisBottom(0, 1., 0., 0.);
@@ -150,6 +151,7 @@ struct btMultibodyLink
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