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

40
examples/Constraints/TestHingeTorque.cpp
View File

@@ -4,12 +4,15 @@
#include "../CommonInterfaces/CommonRigidBodyBase.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter);
short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::StaticFilter|btBroadphaseProxy::CharacterFilter));
struct TestHingeTorque : public CommonRigidBodyBase
{
bool m_once;
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
TestHingeTorque(struct GUIHelperInterface* helper);
virtual ~ TestHingeTorque();
virtual void initPhysics();
@@ -37,6 +40,11 @@ m_once(true)
}
TestHingeTorque::~ TestHingeTorque()
{
for (int i=0;i<m_jointFeedback.size();i++)
{
delete m_jointFeedback[i];
}
}
@@ -70,6 +78,22 @@ void TestHingeTorque::stepSimulation(float deltaTime)
child->getAngularVelocity()[2]);
for (int i=0;i<m_jointFeedback.size();i++)
{
b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
m_jointFeedback[i]->m_appliedForceBodyA.x(),
m_jointFeedback[i]->m_appliedForceBodyA.y(),
m_jointFeedback[i]->m_appliedForceBodyA.z(),
m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
m_jointFeedback[i]->m_appliedTorqueBodyA.y(),
m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
m_jointFeedback[i]->m_appliedForceBodyB.x(),
m_jointFeedback[i]->m_appliedForceBodyB.y(),
m_jointFeedback[i]->m_appliedForceBodyB.z(),
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
}
//CommonRigidBodyBase::stepSimulation(deltaTime);
}
@@ -77,10 +101,10 @@ void TestHingeTorque::stepSimulation(float deltaTime)
void TestHingeTorque::initPhysics()
{
m_guiHelper->setUpAxis(2);
m_guiHelper->setUpAxis(1);
createEmptyDynamicsWorld();
m_dynamicsWorld->setGravity(btVector3(0,0,0));
// m_dynamicsWorld->setGravity(btVector3(0,0,0));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
int mode = btIDebugDraw::DBG_DrawWireframe
@@ -107,13 +131,13 @@ void TestHingeTorque::initPhysics()
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
float baseMass = 0.f;
float linkMass = 1.f;
btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox);
m_dynamicsWorld->removeRigidBody(base);
base->setDamping(0,0);
m_dynamicsWorld->addRigidBody(base,0,0);
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
btBoxShape* linkBox = new btBoxShape(linkHalfExtents);
btRigidBody* prevBody = base;
@@ -126,7 +150,7 @@ void TestHingeTorque::initPhysics()
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox);
m_dynamicsWorld->removeRigidBody(linkBody);
m_dynamicsWorld->addRigidBody(linkBody,0,0);
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
linkBody->setDamping(0,0);
//create a hinge constraint
btVector3 pivotInA(0,-linkHalfExtents[1],0);
@@ -137,6 +161,10 @@ void TestHingeTorque::initPhysics()
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
pivotInA,pivotInB,
axisInA,axisInB,useReferenceA);
btJointFeedback* fb = new btJointFeedback();
m_jointFeedback.push_back(fb);
hinge->setJointFeedback(fb);
m_dynamicsWorld->addConstraint(hinge,true);
prevBody = linkBody;

134
examples/MultiBody/TestJointTorqueSetup.cpp
View File

@@ -2,12 +2,17 @@
#include "TestJointTorqueSetup.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
struct TestJointTorqueSetup : public CommonMultiBodyBase
{
btMultiBody* m_multiBody;
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
bool m_once;
public:
@@ -43,7 +48,7 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
void TestJointTorqueSetup::initPhysics()
{
int upAxis = 2;
int upAxis = 1;
m_guiHelper->setUpAxis(upAxis);
btVector4 colors[4] =
@@ -68,8 +73,10 @@ void TestJointTorqueSetup::initPhysics()
+btIDebugDraw::DBG_DrawAabb
);//+btIDebugDraw::DBG_DrawConstraintLimits);
//create a static ground object
if (0)
if (1)
{
btVector3 groundHalfExtents(20,20,20);
groundHalfExtents[upAxis]=1.f;
@@ -89,14 +96,14 @@ void TestJointTorqueSetup::initPhysics()
}
{
bool floating = true;
bool damping = false;
bool floating = false;
bool damping = true;
bool gyro = false;
int numLinks = 1;
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
bool canSleep = false;
bool selfCollide = false;
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
@@ -107,15 +114,18 @@ void TestJointTorqueSetup::initPhysics()
if(baseMass)
{
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
delete shape;
}
bool isMultiDof = false;
bool isMultiDof = true;
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
m_multiBody = pMultiBody;
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
@@ -123,13 +133,7 @@ void TestJointTorqueSetup::initPhysics()
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
@@ -143,15 +147,46 @@ void TestJointTorqueSetup::initPhysics()
for(int i = 0; i < numLinks; ++i)
{
float linkMass = 1.f;
//if (i==3 || i==2)
// linkMass= 1000;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//new btSphereShape(linkHalfExtents[0]);
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
delete shape;
if(!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
{
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
btQuaternion(0.f, 0.f, 0.f, 1.f),
hingeJointAxis,
parentComToCurrentPivot,
currentPivotToCurrentCom, false);
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
}
else
{
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
}
}
//pMultiBody->finalizeMultiDof();
pMultiBody->finalizeMultiDof();
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
for (int i=0;i<pMultiBody->getNumLinks();i++)
{
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
pMultiBody->getLink(i).m_jointFeedback = fb;
m_jointFeedbacks.push_back(fb);
//break;
}
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
///
@@ -171,7 +206,8 @@ void TestJointTorqueSetup::initPhysics()
}
//
btVector3 gravity(0,0,0);
//gravity[upAxis] = -9.81;
gravity[upAxis] = -9.81;
gravity[0] = 0;
m_dynamicsWorld->setGravity(gravity);
//////////////////////////////////////////////
if(0)//numLinks > 0)
@@ -207,11 +243,11 @@ void TestJointTorqueSetup::initPhysics()
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
@@ -222,7 +258,12 @@ void TestJointTorqueSetup::initPhysics()
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
world->addCollisionObject(col, 2,1+2);
bool isDynamic = (baseMass > 0 && floating);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
btVector3 color(0.0,0.0,0.5);
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
@@ -250,25 +291,33 @@ void TestJointTorqueSetup::initPhysics()
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
m_guiHelper->createCollisionShapeGraphicsObject(box);
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
m_guiHelper->createCollisionShapeGraphicsObject(shape);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
col->setCollisionShape(shape);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
world->addCollisionObject(col,2,1+2);
btVector4 color = colors[curColor];
bool isDynamic = 1;//(linkMass > 0);
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
//if (i==0||i>numLinks-2)
{
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
btVector4 color = colors[curColor];
curColor++;
curColor&=3;
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
pMultiBody->getLink(i).m_collider=col;
}
}
}
@@ -276,7 +325,8 @@ void TestJointTorqueSetup::initPhysics()
void TestJointTorqueSetup::stepSimulation(float deltaTime)
{
if (m_once)
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
if (0)//m_once)
{
m_once=false;
m_multiBody->addJointTorque(0, 10.0);
@@ -286,14 +336,34 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
}
m_dynamicsWorld->stepSimulation(1./60,0);
if (1)
for (int i=0;i<m_jointFeedbacks.size();i++)
{
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
i,
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
);
}
/*
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
m_multiBody->getBaseOmega()[1],
m_multiBody->getBaseOmega()[2]
);
*/
btScalar jointVel =m_multiBody->getJointVel(0);
b3Printf("child angvel = %f",jointVel);
// b3Printf("child angvel = %f",jointVel);