Scarfski/bullet3
1
0
Fork 0
mirror of https://github.com/bulletphysics/bullet3.git synced 2026-08-25 12:38:29 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3145 from WenlongLu/kinematic_multibody_link

Browse Source 
Kinematic Multibody Joint/Link
This commit is contained in:
erwincoumans
2020-11-17 15:20:52 -08:00
committed by GitHub
parent a72ce295b4 8ccd67727f
commit 05b5d2ee04
8 changed files with 275 additions and 144 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
examples/ExampleBrowser/premake4.lua
View File

@@ -196,6 +196,7 @@ project "App_BulletExampleBrowser"
"../MultiBody/MultiBodySoftContact.cpp",
"../MultiBody/MultiBodyConstraintFeedback.cpp",
"../MultiBody/InvertedPendulumPDControl.cpp",
"../MultiBody/KinematicMultiBodyExample.cpp",
"../RigidBody/RigidBodySoftContact.cpp",
"../RigidBody/KinematicRigidBodyExample.cpp",
"../ThirdPartyLibs/stb_image/stb_image.cpp",

47
examples/MultiBody/KinematicMultiBodyExample.cpp
View File

@@ -37,6 +37,14 @@ void kinematicPreTickCallback(btDynamicsWorld* world, btScalar deltaTime)
btVector3 angularVelocity(0, 0.1, 0);
btTransformUtil::integrateTransform(groundBody->getBaseWorldTransform(), linearVelocity, angularVelocity, deltaTime, predictedTrans);
groundBody->setBaseWorldTransform(predictedTrans);
static float time = 0.0;
time += deltaTime;
double old_joint_pos = groundBody->getJointPos(0);
double joint_pos = 0.5 * sin(time * 3.0 - 0.3);
double joint_vel = (joint_pos - old_joint_pos) / deltaTime;
groundBody->setJointPosMultiDof(0, &joint_pos);
groundBody->setJointVelMultiDof(0, &joint_vel);
}
struct KinematicMultiBodyExample : public CommonMultiBodyBase
@@ -79,25 +87,21 @@ void KinematicMultiBodyExample::initPhysics()
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe + btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
btScalar halfExtentsX = 10.0;
btScalar halfExtentsY = 0.1;
btScalar halfExtentsZ = 10.0;
///create a kinematic multibody
btBoxShape* groundShape = createBoxShape(btVector3(btScalar(10.), btScalar(0.1), btScalar(10.)));
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -halfExtentsY, 0));
m_collisionShapes.push_back(groundShape);
btBoxShape* secondLevelShape = createBoxShape(btVector3(btScalar(0.5), btScalar(0.1), btScalar(0.5)));
m_collisionShapes.push_back(secondLevelShape);
{
bool floating = false;
int numLinks = 0;
int numLinks = 1;
bool canSleep = false;
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
btVector3 secondLevelInertiaDiag(0.f, 0.f, 0.f);
float secondLevelMass = 0.1f;
if (baseMass)
{
@@ -105,25 +109,42 @@ void KinematicMultiBodyExample::initPhysics()
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
if (secondLevelMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(0.5, 0.5, 0.5));
pTempBox->calculateLocalInertia(secondLevelMass, secondLevelInertiaDiag);
delete pTempBox;
}
btTransform startTransform;
startTransform.setIdentity();
m_groundBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep);
m_groundBody->setBasePos(startTransform.getOrigin());
m_groundBody->setWorldToBaseRot(startTransform.getRotation());
//init the child link - second level.
btVector3 hingeJointAxis(0, 1, 0);
m_groundBody->setupRevolute(0, secondLevelMass, secondLevelInertiaDiag, -1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, btVector3(0, 0.5, 0), btVector3(0, 0, 0), true);
m_groundBody->finalizeMultiDof();
m_dynamicsWorld->addMultiBody(m_groundBody);
// add collision geometries
bool isDynamic = false; // Kinematic is not treated as dynamic here.
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(m_groundBody, -1);
col->setCollisionShape(groundShape);
bool isDynamic = (baseMass > 0 && floating);
int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
m_dynamicsWorld->addCollisionObject(col, collisionFilterGroup, collisionFilterMask); //, 2,1+2);
m_groundBody->setBaseCollider(col);
m_groundBody->setBaseDynamicType(btCollisionObject::CF_KINEMATIC_OBJECT);
btMultiBodyLinkCollider* secondLevelCol = new btMultiBodyLinkCollider(m_groundBody, 0);
secondLevelCol->setCollisionShape(secondLevelShape);
m_dynamicsWorld->addCollisionObject(secondLevelCol, collisionFilterGroup, collisionFilterMask);
m_groundBody->getLink(0).m_collider = secondLevelCol;
m_groundBody->setLinkDynamicType(0, btCollisionObject::CF_KINEMATIC_OBJECT);
}
m_dynamicsWorld->setInternalTickCallback(kinematicPreTickCallback, m_groundBody, true);

1
src/BulletDynamics/CMakeLists.txt
View File

@@ -38,6 +38,7 @@ SET(BulletDynamics_SRCS
Featherstone/btMultiBodyGearConstraint.cpp
Featherstone/btMultiBodyJointLimitConstraint.cpp
Featherstone/btMultiBodyJointMotor.cpp
Featherstone/btMultiBodyLink.cpp
Featherstone/btMultiBodyMLCPConstraintSolver.cpp
Featherstone/btMultiBodyPoint2Point.cpp
Featherstone/btMultiBodySliderConstraint.cpp

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

@@ -898,31 +898,53 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// calculate zhat_i^A
//
//external forces
btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
if (isLinkStaticOrKinematic(i))
{
zeroAccSpatFrc[i].setZero();
}
else{
//external forces
btVector3 linkAppliedForce = isConstraintPass ? m_links[i].m_appliedConstraintForce : m_links[i].m_appliedForce;
btVector3 linkAppliedTorque = isConstraintPass ? m_links[i].m_appliedConstraintTorque : m_links[i].m_appliedTorque;
zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
zeroAccSpatFrc[i + 1].setVector(-(rot_from_world[i + 1] * linkAppliedTorque), -(rot_from_world[i + 1] * linkAppliedForce));
#if 0
{
{
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
i+1,
zeroAccSpatFrc[i+1].m_topVec[0],
zeroAccSpatFrc[i+1].m_topVec[1],
zeroAccSpatFrc[i+1].m_topVec[2],
b3Printf("stepVelocitiesMultiDof zeroAccSpatFrc[%d] linear:%f,%f,%f, angular:%f,%f,%f",
i+1,
zeroAccSpatFrc[i+1].m_topVec[0],
zeroAccSpatFrc[i+1].m_topVec[1],
zeroAccSpatFrc[i+1].m_topVec[2],
zeroAccSpatFrc[i+1].m_bottomVec[0],
zeroAccSpatFrc[i+1].m_bottomVec[1],
zeroAccSpatFrc[i+1].m_bottomVec[2]);
}
zeroAccSpatFrc[i+1].m_bottomVec[0],
zeroAccSpatFrc[i+1].m_bottomVec[1],
zeroAccSpatFrc[i+1].m_bottomVec[2]);
}
#endif
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
//
//adding damping terms (only)
btScalar linDampMult = 1., angDampMult = 1.;
zeroAccSpatFrc[i + 1].addVector(angDampMult * m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular() * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR * spatVel[i + 1].getAngular().safeNorm()),
linDampMult * m_links[i].m_mass * spatVel[i + 1].getLinear() * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR * spatVel[i + 1].getLinear().safeNorm()));
//p += vhat x Ihat vhat - done in a simpler way
if (m_useGyroTerm)
zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
//
zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
//
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
////clamp parent's omega
//btScalar parOmegaMod = temp.length();
//btScalar parOmegaModMax = 1000;
//if(parOmegaMod > parOmegaModMax)
// temp *= parOmegaModMax / parOmegaMod;
//zeroAccSpatFrc[i+1].addLinear(temp);
//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
//temp = spatCoriolisAcc[i].getLinear();
//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
}
// calculate Ihat_i^A
//init the spatial AB inertia (it has the simple form thanks to choosing local body frames origins at their COMs)
@@ -935,22 +957,6 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
btMatrix3x3(m_links[i].m_inertiaLocal[0], 0, 0,
0, m_links[i].m_inertiaLocal[1], 0,
0, 0, m_links[i].m_inertiaLocal[2]));
//
//p += vhat x Ihat vhat - done in a simpler way
if (m_useGyroTerm)
zeroAccSpatFrc[i + 1].addAngular(spatVel[i + 1].getAngular().cross(m_links[i].m_inertiaLocal * spatVel[i + 1].getAngular()));
//
zeroAccSpatFrc[i + 1].addLinear(m_links[i].m_mass * spatVel[i + 1].getAngular().cross(spatVel[i + 1].getLinear()));
//btVector3 temp = m_links[i].m_mass * spatVel[i+1].getAngular().cross(spatVel[i+1].getLinear());
////clamp parent's omega
//btScalar parOmegaMod = temp.length();
//btScalar parOmegaModMax = 1000;
//if(parOmegaMod > parOmegaModMax)
// temp *= parOmegaModMax / parOmegaMod;
//zeroAccSpatFrc[i+1].addLinear(temp);
//printf("|zeroAccSpatFrc[%d]| = %.4f\n", i+1, temp.length());
//temp = spatCoriolisAcc[i].getLinear();
//printf("|spatCoriolisAcc[%d]| = %.4f\n", i+1, temp.length());
//printf("w[%d] = [%.4f %.4f %.4f]\n", i, vel_top_angular[i+1].x(), vel_top_angular[i+1].y(), vel_top_angular[i+1].z());
//printf("v[%d] = [%.4f %.4f %.4f]\n", i, vel_bottom_linear[i+1].x(), vel_bottom_linear[i+1].y(), vel_bottom_linear[i+1].z());
@@ -961,6 +967,8 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i)
{
if(isLinkStaticOrKinematic(i))
continue;
const int parent = m_links[i].m_parent;
fromParent.m_rotMat = rot_from_parent[i + 1];
fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1107,22 +1115,24 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar
fromParent.transform(spatAcc[parent + 1], spatAcc[i + 1]);
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
if(!isLinkStaticOrKinematic(i))
{
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
{
const btSpatialForceVector &hDof = h[m_links[i].m_dofOffset + dof];
//
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - spatAcc[i + 1].dot(hDof);
}
btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
//D^{-1} * (Y - h^{T}*apar)
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
spatAcc[i + 1] += spatCoriolisAcc[i];
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
}
btScalar *invDi = &invD[m_links[i].m_dofOffset * m_links[i].m_dofOffset];
//D^{-1} * (Y - h^{T}*apar)
mulMatrix(invDi, Y_minus_hT_a, m_links[i].m_dofCount, m_links[i].m_dofCount, m_links[i].m_dofCount, 1, &joint_accel[m_links[i].m_dofOffset]);
spatAcc[i + 1] += spatCoriolisAcc[i];
for (int dof = 0; dof < m_links[i].m_dofCount; ++dof)
spatAcc[i + 1] += m_links[i].m_axes[dof] * joint_accel[m_links[i].m_dofOffset + dof];
if (m_links[i].m_jointFeedback)
{
m_internalNeedsJointFeedback = true;
@@ -1477,6 +1487,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
// (part of TreeForwardDynamics in Mirtich.)
for (int i = num_links - 1; i >= 0; --i)
{
if(isLinkStaticOrKinematic(i))
continue;
const int parent = m_links[i].m_parent;
fromParent.m_rotMat = rot_from_parent[i + 1];
fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1533,6 +1545,8 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
// now do the loop over the m_links
for (int i = 0; i < num_links; ++i)
{
if(isLinkStaticOrKinematic(i))
continue;
const int parent = m_links[i].m_parent;
fromParent.m_rotMat = rot_from_parent[i + 1];
fromParent.m_trnVec = m_links[i].m_cachedRVector;
@@ -1676,55 +1690,88 @@ void btMultiBody::predictPositionsMultiDof(btScalar dt)
btScalar *pJointPos;
pJointPos = &m_links[i].m_jointPos_interpolate[0];
btScalar *pJointVel = getJointVelMultiDof(i);
switch (m_links[i].m_jointType)
if(m_links[i].isStaticOrKinematic())
{
switch (m_links[i].m_jointType)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
pJointPos[0] = m_links[i].m_jointPos[0];
break;
}
case btMultibodyLink::eSpherical:
{
for (int j = 0; j < 4; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
break;
}
case btMultibodyLink::ePlanar:
{
for (int j = 0; j < 3; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
break;
}
default:
break;
}
}
else
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
//reset to current pos
pJointPos[0] = m_links[i].m_jointPos[0];
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
//reset to current pos
btScalar *pJointVel = getJointVelMultiDof(i);
for (int j = 0; j < 4; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
btVector3 jointVel;
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri;
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x();
pJointPos[1] = jointOri.y();
pJointPos[2] = jointOri.z();
pJointPos[3] = jointOri.w();
break;
}
case btMultibodyLink::ePlanar:
switch (m_links[i].m_jointType)
{
for (int j = 0; j < 3; ++j)
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
//reset to current pos
pJointPos[0] = m_links[i].m_jointPos[0];
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
//reset to current pos
for (int j = 0; j < 4; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
btVector3 jointVel;
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri;
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x();
pJointPos[1] = jointOri.y();
pJointPos[2] = jointOri.z();
pJointPos[3] = jointOri.w();
break;
}
case btMultibodyLink::ePlanar:
{
for (int j = 0; j < 3; ++j)
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
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);
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
break;
}
default:
{
pJointPos[j] = m_links[i].m_jointPos[j];
}
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);
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
break;
}
default:
{
}
}
@@ -1826,48 +1873,51 @@ 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;
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
switch (m_links[i].m_jointType)
if(!m_links[i].isStaticOrKinematic())
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
//reset to current pos
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
//reset to current pos
btVector3 jointVel;
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri;
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x();
pJointPos[1] = jointOri.y();
pJointPos[2] = jointOri.z();
pJointPos[3] = jointOri.w();
break;
}
case btMultibodyLink::ePlanar:
{
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
btScalar *pJointPos;
pJointPos= (pq ? pq : &m_links[i].m_jointPos[0]);
btScalar *pJointVel = (pqd ? pqd : getJointVelMultiDof(i));
btVector3 q0_coors_qd1qd2 = getJointVelMultiDof(i)[1] * m_links[i].getAxisBottom(1) + getJointVelMultiDof(i)[2] * m_links[i].getAxisBottom(2);
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
break;
}
default:
switch (m_links[i].m_jointType)
{
case btMultibodyLink::ePrismatic:
case btMultibodyLink::eRevolute:
{
//reset to current pos
btScalar jointVel = pJointVel[0];
pJointPos[0] += dt * jointVel;
break;
}
case btMultibodyLink::eSpherical:
{
//reset to current pos
btVector3 jointVel;
jointVel.setValue(pJointVel[0], pJointVel[1], pJointVel[2]);
btQuaternion jointOri;
jointOri.setValue(pJointPos[0], pJointPos[1], pJointPos[2], pJointPos[3]);
pQuatUpdateFun(jointVel, jointOri, false, dt);
pJointPos[0] = jointOri.x();
pJointPos[1] = jointOri.y();
pJointPos[2] = jointOri.z();
pJointPos[3] = jointOri.w();
break;
}
case btMultibodyLink::ePlanar:
{
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);
btVector3 no_q0_coors_qd1qd2 = quatRotate(btQuaternion(m_links[i].getAxisTop(0), pJointPos[0]), q0_coors_qd1qd2);
pJointPos[1] += m_links[i].getAxisBottom(1).dot(no_q0_coors_qd1qd2) * dt;
pJointPos[2] += m_links[i].getAxisBottom(2).dot(no_q0_coors_qd1qd2) * dt;
break;
}
default:
{
}
}
}

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

@@ -716,6 +716,29 @@ public:
btVector3 &top_out, // top part of output vector
btVector3 &bottom_out); // bottom part of output vector
void setLinkDynamicType(const int i, int type)
{
if(i == -1)
{
setBaseDynamicType(type);
}
else if (i >= 0 && i < getNumLinks())
{
m_links[i].setDynamicType(type);
}
}
bool isLinkStaticOrKinematic(const int i) const
{
if(i == -1)
{
return isBaseStaticOrKinematic();
}
else
{
return m_links[i].isStaticOrKinematic();
}
}
private:

31
src/BulletDynamics/Featherstone/btMultiBodyLink.cpp Normal file
View File

@@ -0,0 +1,31 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
This software is provided 'as-is', without any express or implied warranty.
In no event will the authors be held liable for any damages arising from the use of this software.
Permission is granted to anyone to use this software for any purpose,
including commercial applications, and to alter it and redistribute it freely,
subject to the following restrictions:
1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
3. This notice may not be removed or altered from any source distribution.
*/
#include "btMultiBodyLink.h"
#include "btMultiBodyLinkCollider.h"
bool btMultibodyLink::isStaticOrKinematic() const
{
return m_collider && m_collider->isStaticOrKinematicObject();
}
void btMultibodyLink::setDynamicType(int dynamicType)
{
if(m_collider) {
int oldFlags = m_collider->getCollisionFlags();
oldFlags &= ~(btCollisionObject::CF_STATIC_OBJECT | btCollisionObject::CF_KINEMATIC_OBJECT);
m_collider->setCollisionFlags(oldFlags | dynamicType);
}
}

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

@@ -295,6 +295,9 @@ struct btMultibodyLink
}
}
}
bool isStaticOrKinematic() const;
void setDynamicType(int dynamicType);
};
#endif //BT_MULTIBODY_LINK_H

1
src/btBulletDynamicsAll.cpp
View File

@@ -30,6 +30,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyGearConstraint.cpp"
#include "BulletDynamics/Featherstone/btMultiBodyConstraint.cpp"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.cpp"
#include "BulletDynamics/Featherstone/btMultiBodyLink.cpp"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.cpp"