Merge pull request #410 from erwincoumans/master
support joint force/torque feedback for btMultiBody internal joints (mobilizers)
This commit is contained in:
erwincoumans
181
examples/Constraints/TestHingeTorque.cpp
Normal file
181
examples/Constraints/TestHingeTorque.cpp
Normal file
@@ -0,0 +1,181 @@
|
||||
#include "TestHingeTorque.h"
|
||||
|
||||
|
||||
#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();
|
||||
|
||||
virtual void stepSimulation(float deltaTime);
|
||||
|
||||
|
||||
virtual void resetCamera()
|
||||
{
|
||||
|
||||
float dist = 5;
|
||||
float pitch = 270;
|
||||
float yaw = 21;
|
||||
float targetPos[3]={-1.34,3.4,-0.44};
|
||||
m_guiHelper->resetCamera(dist,pitch,yaw,targetPos[0],targetPos[1],targetPos[2]);
|
||||
}
|
||||
|
||||
|
||||
};
|
||||
|
||||
TestHingeTorque::TestHingeTorque(struct GUIHelperInterface* helper)
|
||||
:CommonRigidBodyBase(helper),
|
||||
m_once(true)
|
||||
{
|
||||
}
|
||||
TestHingeTorque::~ TestHingeTorque()
|
||||
{
|
||||
for (int i=0;i<m_jointFeedback.size();i++)
|
||||
{
|
||||
delete m_jointFeedback[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
void TestHingeTorque::stepSimulation(float deltaTime)
|
||||
{
|
||||
if (m_once)
|
||||
{
|
||||
m_once=false;
|
||||
btHingeConstraint* hinge = (btHingeConstraint*)m_dynamicsWorld->getConstraint(0);
|
||||
|
||||
btRigidBody& bodyA = hinge->getRigidBodyA();
|
||||
btTransform trA = bodyA.getWorldTransform();
|
||||
btVector3 hingeAxisInWorld = trA.getBasis()*hinge->getFrameOffsetA().getBasis().getColumn(2);
|
||||
hinge->getRigidBodyA().applyTorque(-hingeAxisInWorld*10);
|
||||
hinge->getRigidBodyB().applyTorque(hingeAxisInWorld*10);
|
||||
|
||||
}
|
||||
|
||||
m_dynamicsWorld->stepSimulation(1./60,0);
|
||||
btRigidBody* base = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[0]);
|
||||
|
||||
b3Printf("base angvel = %f,%f,%f",base->getAngularVelocity()[0],
|
||||
base->getAngularVelocity()[1],
|
||||
|
||||
base->getAngularVelocity()[2]);
|
||||
|
||||
btRigidBody* child = btRigidBody::upcast(m_dynamicsWorld->getCollisionObjectArray()[1]);
|
||||
|
||||
b3Printf("child angvel = %f,%f,%f",child->getAngularVelocity()[0],
|
||||
child->getAngularVelocity()[1],
|
||||
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
|
||||
void TestHingeTorque::initPhysics()
|
||||
{
|
||||
m_guiHelper->setUpAxis(1);
|
||||
|
||||
createEmptyDynamicsWorld();
|
||||
// m_dynamicsWorld->setGravity(btVector3(0,0,0));
|
||||
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
int mode = btIDebugDraw::DBG_DrawWireframe
|
||||
+btIDebugDraw::DBG_DrawConstraints
|
||||
+btIDebugDraw::DBG_DrawConstraintLimits;
|
||||
m_dynamicsWorld->getDebugDrawer()->setDebugMode(mode);
|
||||
|
||||
|
||||
{ // create a door using hinge constraint attached to the world
|
||||
|
||||
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 baseHalfExtents(0.05, 0.37, 0.1);
|
||||
|
||||
btBoxShape* baseBox = new btBoxShape(baseHalfExtents);
|
||||
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||
btTransform baseWorldTrans;
|
||||
baseWorldTrans.setIdentity();
|
||||
baseWorldTrans.setOrigin(basePosition);
|
||||
|
||||
//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 = 0.f;
|
||||
float linkMass = 1.f;
|
||||
|
||||
btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox);
|
||||
m_dynamicsWorld->removeRigidBody(base);
|
||||
base->setDamping(0,0);
|
||||
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
|
||||
btBoxShape* linkBox = new btBoxShape(linkHalfExtents);
|
||||
btRigidBody* prevBody = base;
|
||||
|
||||
for (int i=0;i<numLinks;i++)
|
||||
{
|
||||
btTransform linkTrans;
|
||||
linkTrans = baseWorldTrans;
|
||||
|
||||
linkTrans.setOrigin(basePosition-btVector3(0,linkHalfExtents[1]*2.f*(i+1),0));
|
||||
|
||||
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox);
|
||||
m_dynamicsWorld->removeRigidBody(linkBody);
|
||||
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
|
||||
linkBody->setDamping(0,0);
|
||||
//create a hinge constraint
|
||||
btVector3 pivotInA(0,-linkHalfExtents[1],0);
|
||||
btVector3 pivotInB(0,linkHalfExtents[1],0);
|
||||
btVector3 axisInA(1,0,0);
|
||||
btVector3 axisInB(1,0,0);
|
||||
bool useReferenceA = true;
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
|
||||
}
|
||||
|
||||
class CommonExampleInterface* TestHingeTorqueCreateFunc(CommonExampleOptions& options)
|
||||
{
|
||||
return new TestHingeTorque(options.m_guiHelper);
|
||||
}
|
||||
7
examples/Constraints/TestHingeTorque.h
Normal file
7
examples/Constraints/TestHingeTorque.h
Normal file
@@ -0,0 +1,7 @@
|
||||
#ifndef TEST_HINGE_TORQUE_H
|
||||
#define TEST_HINGE_TORQUE_H
|
||||
|
||||
class CommonExampleInterface* TestHingeTorqueCreateFunc(struct CommonExampleOptions& options);
|
||||
|
||||
#endif //TEST_HINGE_TORQUE_H
|
||||
|
||||
@@ -97,6 +97,8 @@ SET(App_ExampleBrowser_SRCS
|
||||
../MultiBody/TestJointTorqueSetup.h
|
||||
../MultiBody/MultiDofDemo.cpp
|
||||
../MultiBody/MultiDofDemo.h
|
||||
../Constraints/TestHingeTorque.cpp
|
||||
../Constraints/TestHingeTorque.h
|
||||
../Constraints/ConstraintDemo.cpp
|
||||
../Constraints/ConstraintDemo.h
|
||||
../Constraints/Dof6Spring2Setup.cpp
|
||||
|
||||
@@ -32,6 +32,8 @@
|
||||
#include "../RollingFrictionDemo/RollingFrictionDemo.h"
|
||||
#include "../SharedMemory/PhysicsServer.h"
|
||||
#include "../SharedMemory/PhysicsClient.h"
|
||||
#include "../Constraints/TestHingeTorque.h"
|
||||
|
||||
|
||||
#ifdef ENABLE_LUA
|
||||
#include "../LuaDemo/LuaPhysicsSetup.h"
|
||||
@@ -79,7 +81,9 @@ static ExampleEntry gDefaultExamples[]=
|
||||
AllConstraintCreateFunc),
|
||||
|
||||
ExampleEntry(1,"Motorized Hinge","Use of a btHingeConstraint. You can adjust the first slider to change the target velocity, and the second slider to adjust the maximum impulse applied to reach the target velocity. Note that the hinge angle can reach beyond -360 and 360 degrees.", ConstraintCreateFunc),
|
||||
|
||||
ExampleEntry(1,"TestHingeTorque", "Apply a torque in the hinge axis. This example uses a btHingeConstraint and btRigidBody. The setup is similar to the multi body example TestJointTorque.",
|
||||
TestHingeTorqueCreateFunc),
|
||||
|
||||
// ExampleEntry(0,"What's new in 2.83"),
|
||||
|
||||
ExampleEntry(1,"6DofSpring2","Show the use of the btGeneric6DofSpring2Constraint. This is a replacement of the btGeneric6DofSpringConstraint, it has various improvements. This includes improved spring implementation and better control over the restitution (bounce) when the constraint hits its limits.",
|
||||
@@ -94,7 +98,7 @@ static ExampleEntry gDefaultExamples[]=
|
||||
|
||||
ExampleEntry(0,"MultiBody"),
|
||||
ExampleEntry(1,"MultiDofCreateFunc","Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
|
||||
ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers).", TestJointTorqueCreateFunc),
|
||||
ExampleEntry(1,"TestJointTorque","Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
|
||||
|
||||
|
||||
#ifdef INCLUDE_CLOTH_DEMOS
|
||||
|
||||
@@ -2,13 +2,18 @@
|
||||
#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:
|
||||
|
||||
TestJointTorqueSetup(struct GUIHelperInterface* helper);
|
||||
@@ -31,7 +36,8 @@ public:
|
||||
};
|
||||
|
||||
TestJointTorqueSetup::TestJointTorqueSetup(struct GUIHelperInterface* helper)
|
||||
:CommonMultiBodyBase(helper)
|
||||
:CommonMultiBodyBase(helper),
|
||||
m_once(true)
|
||||
{
|
||||
}
|
||||
|
||||
@@ -42,7 +48,7 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
|
||||
|
||||
void TestJointTorqueSetup::initPhysics()
|
||||
{
|
||||
int upAxis = 2;
|
||||
int upAxis = 1;
|
||||
m_guiHelper->setUpAxis(upAxis);
|
||||
|
||||
btVector4 colors[4] =
|
||||
@@ -67,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;
|
||||
@@ -90,12 +98,12 @@ void TestJointTorqueSetup::initPhysics()
|
||||
{
|
||||
bool floating = false;
|
||||
bool damping = true;
|
||||
bool gyro = true;
|
||||
int numLinks = 5;
|
||||
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);
|
||||
@@ -106,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);
|
||||
@@ -122,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
|
||||
@@ -142,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;
|
||||
|
||||
///
|
||||
@@ -170,10 +206,11 @@ void TestJointTorqueSetup::initPhysics()
|
||||
}
|
||||
//
|
||||
btVector3 gravity(0,0,0);
|
||||
//gravity[upAxis] = -9.81;
|
||||
gravity[upAxis] = -9.81;
|
||||
gravity[0] = 0;
|
||||
m_dynamicsWorld->setGravity(gravity);
|
||||
//////////////////////////////////////////////
|
||||
if(numLinks > 0)
|
||||
if(0)//numLinks > 0)
|
||||
{
|
||||
btScalar q0 = 45.f * SIMD_PI/ 180.f;
|
||||
if(!spherical)
|
||||
@@ -206,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();
|
||||
@@ -221,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);
|
||||
@@ -249,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;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@@ -275,8 +325,48 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
void TestJointTorqueSetup::stepSimulation(float deltaTime)
|
||||
{
|
||||
m_multiBody->addJointTorque(0, 10.0);
|
||||
m_dynamicsWorld->stepSimulation(deltaTime);
|
||||
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
|
||||
if (0)//m_once)
|
||||
{
|
||||
m_once=false;
|
||||
m_multiBody->addJointTorque(0, 10.0);
|
||||
|
||||
btScalar torque = m_multiBody->getJointTorque(0);
|
||||
b3Printf("t = %f,%f,%f\n",torque,torque,torque);//[0],torque[1],torque[2]);
|
||||
}
|
||||
|
||||
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);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
|
||||
|
||||
@@ -24,8 +24,10 @@
|
||||
#include "btMultiBody.h"
|
||||
#include "btMultiBodyLink.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
#include "btMultiBodyJointFeedback.h"
|
||||
#include "LinearMath/btTransformUtil.h"
|
||||
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
// #define INCLUDE_GYRO_TERM
|
||||
|
||||
namespace {
|
||||
@@ -110,7 +112,8 @@ btMultiBody::btMultiBody(int n_links,
|
||||
m_dofCount(0),
|
||||
m_posVarCnt(0),
|
||||
m_useRK4(false),
|
||||
m_useGlobalVelocities(false)
|
||||
m_useGlobalVelocities(false),
|
||||
m_internalNeedsJointFeedback(false)
|
||||
{
|
||||
|
||||
if(!m_isMultiDof)
|
||||
@@ -298,7 +301,6 @@ void btMultiBody::setupSpherical(int i,
|
||||
updateLinksDofOffsets();
|
||||
}
|
||||
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
void btMultiBody::setupPlanar(int i,
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
@@ -347,7 +349,6 @@ void btMultiBody::setupPlanar(int i,
|
||||
//
|
||||
updateLinksDofOffsets();
|
||||
}
|
||||
#endif
|
||||
|
||||
void btMultiBody::finalizeMultiDof()
|
||||
{
|
||||
@@ -629,7 +630,6 @@ inline btMatrix3x3 outerProduct(const btVector3& v0, const btVector3& v1) //r
|
||||
#define vecMulVecTranspose(v0, v1Transposed) outerProduct(v0, v1Transposed)
|
||||
//
|
||||
|
||||
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
btAlignedObjectArray<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
@@ -645,451 +645,8 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
||||
// num_links joint acceleration values.
|
||||
|
||||
int num_links = getNumLinks();
|
||||
m_internalNeedsJointFeedback = false;
|
||||
|
||||
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
|
||||
const btScalar DAMPING_K2_LINEAR = m_linearDamping;
|
||||
|
||||
const btScalar DAMPING_K1_ANGULAR = m_angularDamping;
|
||||
const btScalar DAMPING_K2_ANGULAR= m_angularDamping;
|
||||
|
||||
btVector3 base_vel = getBaseVel();
|
||||
btVector3 base_omega = getBaseOmega();
|
||||
|
||||
// Temporary matrices/vectors -- use scratch space from caller
|
||||
// so that we don't have to keep reallocating every frame
|
||||
|
||||
scratch_r.resize(2*m_dofCount + 6); //multidof? ("Y"s use it and it is used to store qdd) => 2 x m_dofCount
|
||||
scratch_v.resize(8*num_links + 6);
|
||||
scratch_m.resize(4*num_links + 4);
|
||||
|
||||
btScalar * r_ptr = &scratch_r[0];
|
||||
btScalar * output = &scratch_r[m_dofCount]; // "output" holds the q_double_dot results
|
||||
btVector3 * v_ptr = &scratch_v[0];
|
||||
|
||||
// vhat_i (top = angular, bottom = linear part)
|
||||
btVector3 * vel_top_angular = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// zhat_i^A
|
||||
btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// chat_i (note NOT defined for the base)
|
||||
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
|
||||
btVector3 * coriolis_bottom_linear = v_ptr; v_ptr += num_links;
|
||||
|
||||
// top left, top right and bottom left blocks of Ihat_i^A.
|
||||
// bottom right block = transpose of top left block and is not stored.
|
||||
// Note: the top right and bottom left blocks are always symmetric matrices, but we don't make use of this fact currently.
|
||||
btMatrix3x3 * inertia_top_left = &scratch_m[num_links + 1];
|
||||
btMatrix3x3 * inertia_top_right = &scratch_m[2*num_links + 2];
|
||||
btMatrix3x3 * inertia_bottom_left = &scratch_m[3*num_links + 3];
|
||||
|
||||
// Cached 3x3 rotation matrices from parent frame to this frame.
|
||||
btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
||||
btMatrix3x3 * rot_from_world = &scratch_m[0];
|
||||
|
||||
// hhat_i, ahat_i
|
||||
// hhat is NOT stored for the base (but ahat is)
|
||||
btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
|
||||
btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
|
||||
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// Y_i, invD_i
|
||||
btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
||||
btScalar * Y = &scratch_r[0];
|
||||
/////////////////
|
||||
|
||||
// ptr to the joint accel part of the output
|
||||
btScalar * joint_accel = output + 6;
|
||||
|
||||
// Start of the algorithm proper.
|
||||
|
||||
// First 'upward' loop.
|
||||
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
||||
|
||||
rot_from_parent[0] = btMatrix3x3(m_baseQuat); //m_baseQuat assumed to be alias!?
|
||||
|
||||
vel_top_angular[0] = rot_from_parent[0] * base_omega;
|
||||
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
|
||||
|
||||
if (m_fixedBase)
|
||||
{
|
||||
zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
|
||||
}
|
||||
else
|
||||
{
|
||||
zeroAccForce[0] = - (rot_from_parent[0] * (m_baseForce
|
||||
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
|
||||
|
||||
zeroAccTorque[0] =
|
||||
- (rot_from_parent[0] * m_baseTorque);
|
||||
|
||||
if (m_useGyroTerm)
|
||||
zeroAccTorque[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
||||
|
||||
zeroAccTorque[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
||||
|
||||
//NOTE: Coriolis terms are missing! (left like so following Stephen's code)
|
||||
}
|
||||
|
||||
inertia_top_left[0] = btMatrix3x3(0,0,0,0,0,0,0,0,0);
|
||||
|
||||
|
||||
inertia_top_right[0].setValue(m_baseMass, 0, 0,
|
||||
0, m_baseMass, 0,
|
||||
0, 0, m_baseMass);
|
||||
inertia_bottom_left[0].setValue(m_baseInertia[0], 0, 0,
|
||||
0, m_baseInertia[1], 0,
|
||||
0, 0, m_baseInertia[2]);
|
||||
|
||||
rot_from_world[0] = rot_from_parent[0];
|
||||
|
||||
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);
|
||||
|
||||
|
||||
rot_from_world[i+1] = rot_from_parent[i+1] * rot_from_world[parent+1];
|
||||
|
||||
// vhat_i = i_xhat_p(i) * vhat_p(i)
|
||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
vel_top_angular[parent+1], vel_bottom_linear[parent+1],
|
||||
vel_top_angular[i+1], vel_bottom_linear[i+1]);
|
||||
//////////////////////////////////////////////////////////////
|
||||
|
||||
// now set vhat_i to its true value by doing
|
||||
// vhat_i += qidot * shat_i
|
||||
btVector3 joint_vel_spat_top, joint_vel_spat_bottom;
|
||||
joint_vel_spat_top.setZero(); joint_vel_spat_bottom.setZero();
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
joint_vel_spat_top += getJointVelMultiDof(i)[dof] * m_links[i].getAxisTop(dof);
|
||||
joint_vel_spat_bottom += getJointVelMultiDof(i)[dof] * m_links[i].getAxisBottom(dof);
|
||||
}
|
||||
|
||||
vel_top_angular[i+1] += joint_vel_spat_top;
|
||||
vel_bottom_linear[i+1] += joint_vel_spat_bottom;
|
||||
|
||||
// we can now calculate chat_i
|
||||
// remember vhat_i is really vhat_p(i) (but in current frame) at this point
|
||||
SpatialCrossProduct(vel_top_angular[i+1], vel_bottom_linear[i+1], joint_vel_spat_top, joint_vel_spat_bottom, coriolis_top_angular[i], coriolis_bottom_linear[i]);
|
||||
|
||||
// calculate zhat_i^A
|
||||
//
|
||||
//external forces
|
||||
zeroAccForce[i+1] = -(rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
||||
zeroAccTorque[i+1] = -(rot_from_world[i+1] * m_links[i].m_appliedTorque);
|
||||
//
|
||||
//DAMPING TERMS (ONLY)
|
||||
zeroAccForce[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
||||
zeroAccTorque[i+1] += m_links[i].m_inertia * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
||||
|
||||
// calculate Ihat_i^A
|
||||
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
||||
inertia_top_right[i+1].setValue(m_links[i].m_mass, 0, 0,
|
||||
0, m_links[i].m_mass, 0,
|
||||
0, 0, m_links[i].m_mass);
|
||||
inertia_bottom_left[i+1].setValue(m_links[i].m_inertia[0], 0, 0,
|
||||
0, m_links[i].m_inertia[1], 0,
|
||||
0, 0, m_links[i].m_inertia[2]);
|
||||
|
||||
|
||||
////
|
||||
//p += v x* Iv - done in a simpler way
|
||||
if(m_useGyroTerm)
|
||||
zeroAccTorque[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertia * vel_top_angular[i+1] );
|
||||
//
|
||||
coriolis_bottom_linear[i] += vel_top_angular[i+1].cross(vel_bottom_linear[i+1]) - (rot_from_parent[i+1] * (vel_top_angular[parent+1].cross(vel_bottom_linear[parent+1])));
|
||||
//coriolis_bottom_linear[i].setZero();
|
||||
|
||||
//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());
|
||||
//printf("c[%d] = [%.4f %.4f %.4f]\n", i, coriolis_bottom_linear[i].x(), coriolis_bottom_linear[i].y(), coriolis_bottom_linear[i].z());
|
||||
}
|
||||
|
||||
static btScalar D[36]; //it's dofxdof for each body so asingle 6x6 D matrix will do
|
||||
// 'Downward' loop.
|
||||
// (part of TreeForwardDynamics in Mirtich.)
|
||||
for (int i = num_links - 1; i >= 0; --i)
|
||||
{
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
//pFunMultSpatVecTimesSpatMat2(m_links[i].m_axesTop[dof], m_links[i].m_axesBottom[dof], inertia_top_left[i+1], inertia_top_right[i+1], inertia_bottom_left[i+1], h_t, h_b);
|
||||
{
|
||||
h_t = inertia_top_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_right[i+1] * m_links[i].getAxisBottom(dof);
|
||||
h_b = inertia_bottom_left[i+1] * m_links[i].getAxisTop(dof) + inertia_top_left[i+1].transpose() * m_links[i].getAxisBottom(dof);
|
||||
}
|
||||
|
||||
btScalar *D_row = &D[dof * m_links[i].m_dofCount];
|
||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
||||
{
|
||||
D_row[dof2] = SpatialDotProduct(m_links[i].getAxisTop(dof2), m_links[i].getAxisBottom(dof2), h_t, h_b);
|
||||
}
|
||||
|
||||
Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof]
|
||||
- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
|
||||
- SpatialDotProduct(h_t, h_b, coriolis_top_angular[i], coriolis_bottom_linear[i])
|
||||
;
|
||||
}
|
||||
|
||||
const int parent = m_links[i].m_parent;
|
||||
|
||||
btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
||||
switch(m_links[i].m_jointType)
|
||||
{
|
||||
case btMultibodyLink::ePrismatic:
|
||||
case btMultibodyLink::eRevolute:
|
||||
{
|
||||
invDi[0] = 1.0f / D[0];
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case btMultibodyLink::ePlanar:
|
||||
#endif
|
||||
{
|
||||
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
||||
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
|
||||
|
||||
//unroll the loop?
|
||||
for(int row = 0; row < 3; ++row)
|
||||
for(int col = 0; col < 3; ++col)
|
||||
invDi[row * 3 + col] = invD3x3[row][col];
|
||||
|
||||
break;
|
||||
}
|
||||
default:
|
||||
{
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
static btVector3 tmp_top[6]; //move to scratch mem or other buffers? (12 btVector3 will suffice)
|
||||
static btVector3 tmp_bottom[6];
|
||||
|
||||
//for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
//{
|
||||
// tmp_top[dof].setZero();
|
||||
// tmp_bottom[dof].setZero();
|
||||
|
||||
// for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
||||
// {
|
||||
// btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
|
||||
// btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
|
||||
// //
|
||||
// tmp_top[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_b;
|
||||
// tmp_bottom[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * h_t;
|
||||
// }
|
||||
//}
|
||||
|
||||
//btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
|
||||
|
||||
//for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
//{
|
||||
// btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
// btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
// TL -= outerProduct(h_t, tmp_top[dof]);
|
||||
// TR -= outerProduct(h_t, tmp_bottom[dof]);
|
||||
// BL -= outerProduct(h_b, tmp_top[dof]);
|
||||
//}
|
||||
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
tmp_top[dof].setZero();
|
||||
tmp_bottom[dof].setZero();
|
||||
|
||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
||||
{
|
||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof2];
|
||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof2];
|
||||
//
|
||||
tmp_top[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_t;
|
||||
tmp_bottom[dof] += invDi[dof2 * m_links[i].m_dofCount + dof] * h_b;
|
||||
}
|
||||
}
|
||||
|
||||
btMatrix3x3 TL = inertia_top_left[i+1], TR = inertia_top_right[i+1], BL = inertia_bottom_left[i+1];
|
||||
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
TL -= outerProduct(h_t, tmp_bottom[dof]);
|
||||
TR -= outerProduct(h_t, tmp_top[dof]);
|
||||
BL -= outerProduct(h_b, tmp_bottom[dof]);
|
||||
}
|
||||
|
||||
|
||||
btMatrix3x3 r_cross;
|
||||
r_cross.setValue(
|
||||
0, -m_links[i].m_cachedRVector[2], m_links[i].m_cachedRVector[1],
|
||||
m_links[i].m_cachedRVector[2], 0, -m_links[i].m_cachedRVector[0],
|
||||
-m_links[i].m_cachedRVector[1], m_links[i].m_cachedRVector[0], 0);
|
||||
|
||||
inertia_top_left[parent+1] += rot_from_parent[i+1].transpose() * ( TL - TR * r_cross ) * rot_from_parent[i+1];
|
||||
inertia_top_right[parent+1] += rot_from_parent[i+1].transpose() * TR * rot_from_parent[i+1];
|
||||
inertia_bottom_left[parent+1] += rot_from_parent[i+1].transpose() *
|
||||
(r_cross * (TL - TR * r_cross) + BL - TL.transpose() * r_cross) * rot_from_parent[i+1];
|
||||
|
||||
|
||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
||||
|
||||
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
invD_times_Y[dof] = 0.f;
|
||||
|
||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
||||
{
|
||||
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
||||
}
|
||||
}
|
||||
|
||||
in_top = zeroAccForce[i+1]
|
||||
+ inertia_top_left[i+1] * coriolis_top_angular[i]
|
||||
+ inertia_top_right[i+1] * coriolis_bottom_linear[i];
|
||||
|
||||
in_bottom = zeroAccTorque[i+1]
|
||||
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
|
||||
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i];
|
||||
|
||||
//unroll the loop?
|
||||
for(int row = 0; row < 3; ++row)
|
||||
{
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
in_top[row] += h_t[row] * invD_times_Y[dof];
|
||||
in_bottom[row] += h_b[row] * invD_times_Y[dof];
|
||||
}
|
||||
}
|
||||
|
||||
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
in_top, in_bottom, out_top, out_bottom);
|
||||
|
||||
zeroAccForce[parent+1] += out_top;
|
||||
zeroAccTorque[parent+1] += out_bottom;
|
||||
}
|
||||
|
||||
|
||||
// Second 'upward' loop
|
||||
// (part of TreeForwardDynamics in Mirtich)
|
||||
|
||||
if (m_fixedBase)
|
||||
{
|
||||
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
||||
}
|
||||
else
|
||||
{
|
||||
if (num_links > 0)
|
||||
{
|
||||
m_cachedInertiaTopLeft = inertia_top_left[0];
|
||||
m_cachedInertiaTopRight = inertia_top_right[0];
|
||||
m_cachedInertiaLowerLeft = inertia_bottom_left[0];
|
||||
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
|
||||
|
||||
}
|
||||
btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
|
||||
btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
|
||||
float result[6];
|
||||
|
||||
solveImatrix(rhs_top, rhs_bot, result);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
accel_top[0][i] = -result[i];
|
||||
accel_bottom[0][i] = -result[i+3];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough
|
||||
// now do the loop over the m_links
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
const int parent = m_links[i].m_parent;
|
||||
|
||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
accel_top[parent+1], accel_bottom[parent+1],
|
||||
accel_top[i+1], accel_bottom[i+1]);
|
||||
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
|
||||
}
|
||||
|
||||
btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
||||
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]);
|
||||
|
||||
accel_top[i+1] += coriolis_top_angular[i];
|
||||
accel_bottom[i+1] += coriolis_bottom_linear[i];
|
||||
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
|
||||
accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
|
||||
}
|
||||
}
|
||||
|
||||
// transform base accelerations back to the world frame.
|
||||
btVector3 omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
|
||||
output[0] = omegadot_out[0];
|
||||
output[1] = omegadot_out[1];
|
||||
output[2] = omegadot_out[2];
|
||||
|
||||
btVector3 vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
|
||||
output[3] = vdot_out[0];
|
||||
output[4] = vdot_out[1];
|
||||
output[5] = vdot_out[2];
|
||||
|
||||
/////////////////
|
||||
//printf("q = [");
|
||||
//printf("%.6f, %.6f, %.6f, %.6f, %.6f, %.6f, %.6f ", m_baseQuat.x(), m_baseQuat.y(), m_baseQuat.z(), m_baseQuat.w(), m_basePos.x(), m_basePos.y(), m_basePos.z());
|
||||
//for(int link = 0; link < getNumLinks(); ++link)
|
||||
// for(int dof = 0; dof < m_links[link].m_dofCount; ++dof)
|
||||
// printf("%.6f ", m_links[link].m_jointPos[dof]);
|
||||
//printf("]\n");
|
||||
////
|
||||
//printf("qd = [");
|
||||
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
||||
// printf("%.6f ", m_realBuf[dof]);
|
||||
//printf("]\n");
|
||||
//printf("qdd = [");
|
||||
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
||||
// printf("%.6f ", output[dof]);
|
||||
//printf("]\n");
|
||||
/////////////////
|
||||
|
||||
// Final step: add the accelerations (times dt) to the velocities.
|
||||
applyDeltaVeeMultiDof(output, dt);
|
||||
}
|
||||
|
||||
#else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
|
||||
void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
btAlignedObjectArray<btScalar> &scratch_r,
|
||||
btAlignedObjectArray<btVector3> &scratch_v,
|
||||
btAlignedObjectArray<btMatrix3x3> &scratch_m)
|
||||
{
|
||||
// Implement Featherstone's algorithm to calculate joint accelerations (q_double_dot)
|
||||
// and the base linear & angular accelerations.
|
||||
|
||||
// We apply damping forces in this routine as well as any external forces specified by the
|
||||
// caller (via addBaseForce etc).
|
||||
|
||||
// output should point to an array of 6 + num_links reals.
|
||||
// Format is: 3 angular accelerations (in world frame), 3 linear accelerations (in world frame),
|
||||
// num_links joint acceleration values.
|
||||
|
||||
int num_links = getNumLinks();
|
||||
|
||||
const btScalar DAMPING_K1_LINEAR = m_linearDamping;
|
||||
@@ -1244,6 +801,20 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
//
|
||||
//external forces
|
||||
zeroAccSpatFrc[i+1].setVector(-(rot_from_world[i+1] * m_links[i].m_appliedTorque), -(rot_from_world[i+1] * m_links[i].m_appliedForce));
|
||||
|
||||
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],
|
||||
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[0],
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[1],
|
||||
zeroAccSpatFrc[i+1].m_bottomVec[2]);
|
||||
}
|
||||
//
|
||||
//adding damping terms (only)
|
||||
btScalar linDampMult = 1., angDampMult = 1.;
|
||||
@@ -1325,9 +896,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
break;
|
||||
}
|
||||
case btMultibodyLink::eSpherical:
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case btMultibodyLink::ePlanar:
|
||||
#endif
|
||||
{
|
||||
static btMatrix3x3 D3x3; D3x3.setValue(D[0], D[1], D[2], D[3], D[4], D[5], D[6], D[7], D[8]);
|
||||
static btMatrix3x3 invD3x3; invD3x3 = D3x3.inverse();
|
||||
@@ -1421,6 +990,7 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
spatAcc[0] = -result;
|
||||
}
|
||||
|
||||
|
||||
// now do the loop over the m_links
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
@@ -1450,6 +1020,17 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
|
||||
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;
|
||||
m_links[i].m_jointFeedback->m_spatialInertia = spatInertia[i+1];
|
||||
m_links[i].m_jointFeedback->m_reactionForces.m_bottomVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_bottomVec;
|
||||
m_links[i].m_jointFeedback->m_reactionForces.m_topVec = rot_from_parent[0].transpose()*(spatInertia[i+1]*spatAcc[i+1]+zeroAccSpatFrc[i+1]).m_topVec;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// transform base accelerations back to the world frame.
|
||||
@@ -1546,7 +1127,6 @@ void btMultiBody::stepVelocitiesMultiDof(btScalar dt,
|
||||
}
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
void btMultiBody::stepVelocities(btScalar dt,
|
||||
@@ -1591,8 +1171,8 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
btVector3 * vel_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// zhat_i^A
|
||||
btVector3 * zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * f_zero_acc_top_angular = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * f_zero_acc_bottom_linear = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// chat_i (note NOT defined for the base)
|
||||
btVector3 * coriolis_top_angular = v_ptr; v_ptr += num_links;
|
||||
@@ -1617,7 +1197,7 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// Y_i, D_i
|
||||
btScalar * Y = r_ptr; r_ptr += num_links;
|
||||
btScalar * Y1 = r_ptr; r_ptr += num_links;
|
||||
btScalar * D = num_links > 0 ? &m_realBuf[6 + num_links] : 0;
|
||||
|
||||
// ptr to the joint accel part of the output
|
||||
@@ -1635,18 +1215,18 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
vel_bottom_linear[0] = rot_from_parent[0] * base_vel;
|
||||
|
||||
if (m_fixedBase) {
|
||||
zero_acc_top_angular[0] = zero_acc_bottom_linear[0] = btVector3(0,0,0);
|
||||
f_zero_acc_top_angular[0] = f_zero_acc_bottom_linear[0] = btVector3(0,0,0);
|
||||
} else {
|
||||
zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
|
||||
f_zero_acc_top_angular[0] = - (rot_from_parent[0] * (m_baseForce
|
||||
- m_baseMass*(DAMPING_K1_LINEAR+DAMPING_K2_LINEAR*base_vel.norm())*base_vel));
|
||||
|
||||
zero_acc_bottom_linear[0] =
|
||||
f_zero_acc_bottom_linear[0] =
|
||||
- (rot_from_parent[0] * m_baseTorque);
|
||||
|
||||
if (m_useGyroTerm)
|
||||
zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
||||
f_zero_acc_bottom_linear[0]+=vel_top_angular[0].cross( m_baseInertia * vel_top_angular[0] );
|
||||
|
||||
zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
||||
f_zero_acc_bottom_linear[0] += m_baseInertia * vel_top_angular[0] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[0].norm());
|
||||
|
||||
}
|
||||
|
||||
@@ -1693,17 +1273,17 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
vel_bottom_linear[i+1] += getJointVel(i) * m_links[i].getAxisBottom(0);
|
||||
|
||||
// calculate zhat_i^A
|
||||
zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
||||
zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
||||
f_zero_acc_top_angular[i+1] = - (rot_from_world[i+1] * (m_links[i].m_appliedForce));
|
||||
f_zero_acc_top_angular[i+1] += m_links[i].m_mass * (DAMPING_K1_LINEAR + DAMPING_K2_LINEAR*vel_bottom_linear[i+1].norm()) * vel_bottom_linear[i+1];
|
||||
|
||||
zero_acc_bottom_linear[i+1] =
|
||||
f_zero_acc_bottom_linear[i+1] =
|
||||
- (rot_from_world[i+1] * m_links[i].m_appliedTorque);
|
||||
if (m_useGyroTerm)
|
||||
{
|
||||
zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
|
||||
f_zero_acc_bottom_linear[i+1] += vel_top_angular[i+1].cross( m_links[i].m_inertiaLocal * vel_top_angular[i+1] );
|
||||
}
|
||||
|
||||
zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
||||
f_zero_acc_bottom_linear[i+1] += m_links[i].m_inertiaLocal * vel_top_angular[i+1] * (DAMPING_K1_ANGULAR + DAMPING_K2_ANGULAR*vel_top_angular[i+1].norm());
|
||||
|
||||
// calculate Ihat_i^A
|
||||
inertia_top_left[i+1] = btMatrix3x3(0,0,0,0,0,0,0,0,0);//::Zero();
|
||||
@@ -1724,9 +1304,9 @@ 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])
|
||||
//Y1 = joint torque - zero acceleration force - coriolis force
|
||||
Y1[i] = m_links[i].m_jointTorque[0]
|
||||
- SpatialDotProduct(m_links[i].getAxisTop(0), m_links[i].getAxisBottom(0), f_zero_acc_top_angular[i+1], f_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;
|
||||
@@ -1757,19 +1337,19 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
|
||||
// Zp += pXi * (Zi + Ii*ci + hi*Yi/Di)
|
||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
||||
const btScalar Y_over_D = Y[i] * one_over_di;
|
||||
in_top = zero_acc_top_angular[i+1]
|
||||
const btScalar Y_over_D = Y1[i] * one_over_di;
|
||||
in_top = f_zero_acc_top_angular[i+1]
|
||||
+ inertia_top_left[i+1] * coriolis_top_angular[i]
|
||||
+ inertia_top_right[i+1] * coriolis_bottom_linear[i]
|
||||
+ Y_over_D * h_top[i];
|
||||
in_bottom = zero_acc_bottom_linear[i+1]
|
||||
in_bottom = f_zero_acc_bottom_linear[i+1]
|
||||
+ inertia_bottom_left[i+1] * coriolis_top_angular[i]
|
||||
+ inertia_top_left[i+1].transpose() * coriolis_bottom_linear[i]
|
||||
+ Y_over_D * h_bottom[i];
|
||||
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
in_top, in_bottom, out_top, out_bottom);
|
||||
zero_acc_top_angular[parent+1] += out_top;
|
||||
zero_acc_bottom_linear[parent+1] += out_bottom;
|
||||
f_zero_acc_top_angular[parent+1] += out_top;
|
||||
f_zero_acc_bottom_linear[parent+1] += out_bottom;
|
||||
}
|
||||
|
||||
|
||||
@@ -1797,8 +1377,8 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
m_cachedInertiaLowerRight= inertia_top_left[0].transpose();
|
||||
|
||||
}
|
||||
btVector3 rhs_top (zero_acc_top_angular[0][0], zero_acc_top_angular[0][1], zero_acc_top_angular[0][2]);
|
||||
btVector3 rhs_bot (zero_acc_bottom_linear[0][0], zero_acc_bottom_linear[0][1], zero_acc_bottom_linear[0][2]);
|
||||
btVector3 rhs_top (f_zero_acc_top_angular[0][0], f_zero_acc_top_angular[0][1], f_zero_acc_top_angular[0][2]);
|
||||
btVector3 rhs_bot (f_zero_acc_bottom_linear[0][0], f_zero_acc_bottom_linear[0][1], f_zero_acc_bottom_linear[0][2]);
|
||||
float result[6];
|
||||
|
||||
solveImatrix(rhs_top, rhs_bot, result);
|
||||
@@ -1811,12 +1391,18 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
}
|
||||
|
||||
// now do the loop over the m_links
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
//acceleration of the child link = acceleration of the parent transformed into child frame +
|
||||
// + coriolis acceleration
|
||||
// + joint acceleration
|
||||
const int parent = m_links[i].m_parent;
|
||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
accel_top[parent+1], accel_bottom[parent+1],
|
||||
accel_top[i+1], accel_bottom[i+1]);
|
||||
joint_accel[i] = (Y[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
|
||||
|
||||
|
||||
joint_accel[i] = (Y1[i] - SpatialDotProduct(h_top[i], h_bottom[i], accel_top[i+1], accel_bottom[i+1])) / D[i];
|
||||
accel_top[i+1] += coriolis_top_angular[i] + joint_accel[i] * m_links[i].getAxisTop(0);
|
||||
accel_bottom[i+1] += coriolis_bottom_linear[i] + joint_accel[i] * m_links[i].getAxisBottom(0);
|
||||
}
|
||||
@@ -1851,6 +1437,8 @@ void btMultiBody::stepVelocities(btScalar dt,
|
||||
/////////////////
|
||||
|
||||
// Final step: add the accelerations (times dt) to the velocities.
|
||||
//todo: do we already need to update the velocities, or can we move this into the constraint solver?
|
||||
//the gravity (and other forces) will cause an undesired bounce/restitution effect when using this approach
|
||||
applyDeltaVee(output, dt);
|
||||
|
||||
|
||||
@@ -1904,7 +1492,6 @@ void btMultiBody::solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bo
|
||||
|
||||
}
|
||||
}
|
||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const
|
||||
{
|
||||
int num_links = getNumLinks();
|
||||
@@ -1944,7 +1531,6 @@ void btMultiBody::solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionV
|
||||
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const
|
||||
{
|
||||
@@ -1961,194 +1547,13 @@ void btMultiBody::mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, in
|
||||
}
|
||||
}
|
||||
|
||||
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
|
||||
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
||||
{
|
||||
// Temporary matrices/vectors -- use scratch space from caller
|
||||
// so that we don't have to keep reallocating every frame
|
||||
|
||||
int num_links = getNumLinks();
|
||||
int m_dofCount = getNumDofs();
|
||||
scratch_r.resize(m_dofCount);
|
||||
scratch_v.resize(4*num_links + 4);
|
||||
|
||||
btScalar * r_ptr = m_dofCount ? &scratch_r[0] : 0;
|
||||
btVector3 * v_ptr = &scratch_v[0];
|
||||
|
||||
// zhat_i^A (scratch space)
|
||||
btVector3 * zeroAccForce = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * zeroAccTorque = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// rot_from_parent (cached from calcAccelerations)
|
||||
const btMatrix3x3 * rot_from_parent = &m_matrixBuf[0];
|
||||
|
||||
// hhat (cached), accel (scratch)
|
||||
// hhat is NOT stored for the base (but ahat is)
|
||||
const btVector3 * h_top = m_dofCount > 0 ? &m_vectorBuf[0] : 0;
|
||||
const btVector3 * h_bottom = m_dofCount > 0 ? &m_vectorBuf[m_dofCount] : 0;
|
||||
btVector3 * accel_top = v_ptr; v_ptr += num_links + 1;
|
||||
btVector3 * accel_bottom = v_ptr; v_ptr += num_links + 1;
|
||||
|
||||
// Y_i (scratch), invD_i (cached)
|
||||
const btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0;
|
||||
btScalar * Y = r_ptr;
|
||||
////////////////
|
||||
|
||||
// First 'upward' loop.
|
||||
// Combines CompTreeLinkVelocities and InitTreeLinks from Mirtich.
|
||||
|
||||
btVector3 input_force(force[3],force[4],force[5]);
|
||||
btVector3 input_torque(force[0],force[1],force[2]);
|
||||
|
||||
// Fill in zero_acc
|
||||
// -- set to force/torque on the base, zero otherwise
|
||||
if (m_fixedBase)
|
||||
{
|
||||
zeroAccForce[0] = zeroAccTorque[0] = btVector3(0,0,0);
|
||||
} else
|
||||
{
|
||||
zeroAccForce[0] = - (rot_from_parent[0] * input_force);
|
||||
zeroAccTorque[0] = - (rot_from_parent[0] * input_torque);
|
||||
}
|
||||
for (int i = 0; i < num_links; ++i)
|
||||
{
|
||||
zeroAccForce[i+1] = zeroAccTorque[i+1] = btVector3(0,0,0);
|
||||
}
|
||||
|
||||
// 'Downward' loop.
|
||||
// (part of TreeForwardDynamics in Mirtich.)
|
||||
for (int i = num_links - 1; i >= 0; --i)
|
||||
{
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
//?? btScalar sdp = -SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1]);
|
||||
|
||||
Y[m_links[i].m_dofOffset + dof] = force[6 + m_links[i].m_dofOffset + dof]
|
||||
- SpatialDotProduct(m_links[i].getAxisTop(dof), m_links[i].getAxisBottom(dof), zeroAccForce[i+1], zeroAccTorque[i+1])
|
||||
;
|
||||
}
|
||||
|
||||
btScalar aa = Y[i];
|
||||
const int parent = m_links[i].m_parent;
|
||||
|
||||
btVector3 in_top, in_bottom, out_top, out_bottom;
|
||||
const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
||||
|
||||
static btScalar invD_times_Y[6]; //D^{-1} * Y [dofxdof x dofx1 = dofx1] <=> D^{-1} * u; definitely move to buffers; num_dof of btScalar would cover all bodies but acutally 6 btScalars will also be okay
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
invD_times_Y[dof] = 0.f;
|
||||
|
||||
for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2)
|
||||
{
|
||||
invD_times_Y[dof] += invDi[dof * m_links[i].m_dofCount + dof2] * Y[m_links[i].m_dofOffset + dof2];
|
||||
}
|
||||
}
|
||||
|
||||
// Zp += pXi * (Zi + hi*Yi/Di)
|
||||
in_top = zeroAccForce[i+1];
|
||||
in_bottom = zeroAccTorque[i+1];
|
||||
|
||||
//unroll the loop?
|
||||
for(int row = 0; row < 3; ++row)
|
||||
{
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
in_top[row] += h_t[row] * invD_times_Y[dof];
|
||||
in_bottom[row] += h_b[row] * invD_times_Y[dof];
|
||||
}
|
||||
}
|
||||
|
||||
InverseSpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
in_top, in_bottom, out_top, out_bottom);
|
||||
zeroAccForce[parent+1] += out_top;
|
||||
zeroAccTorque[parent+1] += out_bottom;
|
||||
}
|
||||
|
||||
// ptr to the joint accel part of the output
|
||||
btScalar * joint_accel = output + 6;
|
||||
|
||||
|
||||
// Second 'upward' loop
|
||||
// (part of TreeForwardDynamics in Mirtich)
|
||||
|
||||
if (m_fixedBase)
|
||||
{
|
||||
accel_top[0] = accel_bottom[0] = btVector3(0,0,0);
|
||||
}
|
||||
else
|
||||
{
|
||||
btVector3 rhs_top (zeroAccForce[0][0], zeroAccForce[0][1], zeroAccForce[0][2]);
|
||||
btVector3 rhs_bot (zeroAccTorque[0][0], zeroAccTorque[0][1], zeroAccTorque[0][2]);
|
||||
float result[6];
|
||||
|
||||
solveImatrix(rhs_top, rhs_bot, result);
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
accel_top[0][i] = -result[i];
|
||||
accel_bottom[0][i] = -result[i+3];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
static btScalar Y_minus_hT_a[6]; //it's dofx1 for each body so a single 6x1 temp is enough
|
||||
// now do the loop over the m_links
|
||||
for (int i = 0; i < num_links; ++i) {
|
||||
const int parent = m_links[i].m_parent;
|
||||
|
||||
SpatialTransform(rot_from_parent[i+1], m_links[i].m_cachedRVector,
|
||||
accel_top[parent+1], accel_bottom[parent+1],
|
||||
accel_top[i+1], accel_bottom[i+1]);
|
||||
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
const btVector3 &h_t = h_top[m_links[i].m_dofOffset + dof];
|
||||
const btVector3 &h_b = h_bottom[m_links[i].m_dofOffset + dof];
|
||||
|
||||
Y_minus_hT_a[dof] = Y[m_links[i].m_dofOffset + dof] - SpatialDotProduct(h_t, h_b, accel_top[i+1], accel_bottom[i+1]);
|
||||
}
|
||||
|
||||
const btScalar *invDi = &invD[m_links[i].m_dofOffset*m_links[i].m_dofOffset];
|
||||
mulMatrix(const_cast<btScalar*>(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]);
|
||||
|
||||
for(int dof = 0; dof < m_links[i].m_dofCount; ++dof)
|
||||
{
|
||||
accel_top[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisTop(dof);
|
||||
accel_bottom[i+1] += joint_accel[m_links[i].m_dofOffset + dof] * m_links[i].getAxisBottom(dof);
|
||||
}
|
||||
}
|
||||
|
||||
// transform base accelerations back to the world frame.
|
||||
btVector3 omegadot_out;
|
||||
omegadot_out = rot_from_parent[0].transpose() * accel_top[0];
|
||||
output[0] = omegadot_out[0];
|
||||
output[1] = omegadot_out[1];
|
||||
output[2] = omegadot_out[2];
|
||||
|
||||
btVector3 vdot_out;
|
||||
vdot_out = rot_from_parent[0].transpose() * accel_bottom[0];
|
||||
|
||||
output[3] = vdot_out[0];
|
||||
output[4] = vdot_out[1];
|
||||
output[5] = vdot_out[2];
|
||||
|
||||
/////////////////
|
||||
//printf("delta = [");
|
||||
//for(int dof = 0; dof < getNumDofs() + 6; ++dof)
|
||||
// printf("%.2f ", output[dof]);
|
||||
//printf("]\n");
|
||||
/////////////////
|
||||
}
|
||||
#else //i.e. TEST_SPATIAL_ALGEBRA_LAYER
|
||||
void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output,
|
||||
btAlignedObjectArray<btScalar> &scratch_r, btAlignedObjectArray<btVector3> &scratch_v) const
|
||||
{
|
||||
// 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);
|
||||
@@ -2303,7 +1708,6 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
//printf("]\n");
|
||||
/////////////////
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
|
||||
@@ -2595,7 +1999,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
pJointPos[0] = jointOri.x(); pJointPos[1] = jointOri.y(); pJointPos[2] = jointOri.z(); pJointPos[3] = jointOri.w();
|
||||
break;
|
||||
}
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
pJointPos[0] += dt * getJointVelMultiDof(i)[0];
|
||||
@@ -2607,7 +2010,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
{
|
||||
}
|
||||
@@ -2722,7 +2124,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
|
||||
|
||||
break;
|
||||
}
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case btMultibodyLink::ePlanar:
|
||||
{
|
||||
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));
|
||||
@@ -2731,7 +2132,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
{
|
||||
}
|
||||
@@ -2894,3 +2294,52 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
||||
wakeUp();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
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;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@@ -95,7 +95,6 @@ public:
|
||||
const btVector3 &thisPivotToThisComOffset, // vector from joint axis to my COM, in MY frame
|
||||
bool disableParentCollision=false);
|
||||
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
void setupPlanar(int i, // 0 to num_links-1
|
||||
btScalar mass,
|
||||
const btVector3 &inertia,
|
||||
@@ -104,7 +103,6 @@ public:
|
||||
const btVector3 &rotationAxis,
|
||||
const btVector3 &parentComToThisComOffset, // vector from parent COM to this COM, in PARENT frame
|
||||
bool disableParentCollision=false);
|
||||
#endif
|
||||
|
||||
const btMultibodyLink& getLink(int index) const
|
||||
{
|
||||
@@ -150,6 +148,7 @@ public:
|
||||
btScalar getLinkMass(int i) const;
|
||||
const btVector3 & getLinkInertia(int i) const;
|
||||
|
||||
|
||||
|
||||
//
|
||||
// change mass (incomplete: can only change base mass and inertia at present)
|
||||
@@ -185,6 +184,15 @@ public:
|
||||
setWorldToBaseRot(tr.getRotation().inverse());
|
||||
|
||||
}
|
||||
|
||||
btTransform getBaseWorldTransform() const
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setOrigin(getBasePos());
|
||||
tr.setRotation(getWorldToBaseRot().inverse());
|
||||
return tr;
|
||||
}
|
||||
|
||||
void setBaseVel(const btVector3 &vel)
|
||||
{
|
||||
|
||||
@@ -217,6 +225,8 @@ public:
|
||||
void setJointPosMultiDof(int i, btScalar *q);
|
||||
void setJointVelMultiDof(int i, btScalar *qdot);
|
||||
|
||||
|
||||
|
||||
//
|
||||
// direct access to velocities as a vector of 6 + num_links elements.
|
||||
// (omega first, then v, then joint velocities.)
|
||||
@@ -389,6 +399,8 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// timestep the positions (given current velocities).
|
||||
void stepPositions(btScalar dt);
|
||||
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
|
||||
@@ -536,6 +548,14 @@ public:
|
||||
__posUpdated = updated;
|
||||
}
|
||||
|
||||
//internalNeedsJointFeedback is for internal use only
|
||||
bool internalNeedsJointFeedback() const
|
||||
{
|
||||
return m_internalNeedsJointFeedback;
|
||||
}
|
||||
void forwardKinematics(btAlignedObjectArray<btQuaternion>& scratch_q,btAlignedObjectArray<btVector3>& scratch_m);
|
||||
|
||||
|
||||
private:
|
||||
btMultiBody(const btMultiBody &); // not implemented
|
||||
void operator=(const btMultiBody &); // not implemented
|
||||
@@ -543,9 +563,7 @@ private:
|
||||
void compTreeLinkVelocities(btVector3 *omega, btVector3 *vel) const;
|
||||
|
||||
void solveImatrix(const btVector3& rhs_top, const btVector3& rhs_bot, float result[6]) const;
|
||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
void solveImatrix(const btSpatialForceVector &rhs, btSpatialMotionVector &result) const;
|
||||
#endif
|
||||
|
||||
void updateLinksDofOffsets()
|
||||
{
|
||||
@@ -559,6 +577,7 @@ private:
|
||||
|
||||
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
@@ -622,6 +641,9 @@ private:
|
||||
bool __posUpdated;
|
||||
int m_dofCount, m_posVarCnt;
|
||||
bool m_useRK4, m_useGlobalVelocities;
|
||||
|
||||
///the m_needsJointFeedback gets updated/computed during the stepVelocitiesMultiDof and it for internal usage only
|
||||
bool m_internalNeedsJointFeedback;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@@ -93,6 +93,14 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
if (solverConstraint.m_linkA<0)
|
||||
{
|
||||
rel_pos1 = posAworld - multiBodyA->getBasePos();
|
||||
} else
|
||||
{
|
||||
rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
|
||||
}
|
||||
|
||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
@@ -136,8 +144,12 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
||||
multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
else
|
||||
multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
|
||||
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = contactNormalOnB;
|
||||
}
|
||||
else if(rb0)
|
||||
else //if(rb0)
|
||||
{
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
@@ -147,6 +159,14 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
if (solverConstraint.m_linkB<0)
|
||||
{
|
||||
rel_pos2 = posBworld - multiBodyB->getBasePos();
|
||||
} else
|
||||
{
|
||||
rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
|
||||
}
|
||||
|
||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
@@ -186,8 +206,12 @@ btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstr
|
||||
else
|
||||
multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
|
||||
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -contactNormalOnB;
|
||||
|
||||
}
|
||||
else if(rb1)
|
||||
else //if(rb1)
|
||||
{
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
|
||||
@@ -13,6 +13,7 @@ subject to the following restrictions:
|
||||
3. This notice may not be removed or altered from any source distribution.
|
||||
*/
|
||||
|
||||
|
||||
#include "btMultiBodyConstraintSolver.h"
|
||||
#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h"
|
||||
#include "btMultiBodyLinkCollider.h"
|
||||
@@ -165,12 +166,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse,c.m_deltaVelAindex,ndofA);
|
||||
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
if(c.m_multiBodyA->isMultiDof())
|
||||
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
else
|
||||
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],deltaImpulse);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(c.m_solverBodyIdA >= 0)
|
||||
{
|
||||
bodyA->internalApplyImpulse(c.m_contactNormal1*bodyA->internalGetInvMass(),c.m_angularComponentA,deltaImpulse);
|
||||
@@ -179,12 +182,14 @@ void btMultiBodyConstraintSolver::resolveSingleConstraintRowGeneric(const btMult
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse,c.m_deltaVelBindex,ndofB);
|
||||
#ifdef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
//note: update of the actual velocities (below) in the multibody does not have to happen now since m_deltaVelocities can be applied after all iterations
|
||||
//it would make the multibody solver more like the regular one with m_deltaVelocities being equivalent to btSolverBody::m_deltaLinearVelocity/m_deltaAngularVelocity
|
||||
if(c.m_multiBodyB->isMultiDof())
|
||||
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
else
|
||||
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],deltaImpulse);
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
} else if(c.m_solverBodyIdB >= 0)
|
||||
{
|
||||
bodyB->internalApplyImpulse(c.m_contactNormal2*bodyB->internalGetInvMass(),c.m_angularComponentB,deltaImpulse);
|
||||
@@ -279,8 +284,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
|
||||
relaxation = 1.f;
|
||||
|
||||
|
||||
|
||||
|
||||
if (multiBodyA)
|
||||
{
|
||||
if (solverConstraint.m_linkA<0)
|
||||
{
|
||||
rel_pos1 = pos1 - multiBodyA->getBasePos();
|
||||
} else
|
||||
{
|
||||
rel_pos1 = pos1 - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
|
||||
}
|
||||
const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
|
||||
@@ -310,16 +325,30 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
multiBodyA->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
||||
else
|
||||
multiBodyA->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacAindex],delta,m_data.scratch_r, m_data.scratch_v);
|
||||
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = contactNormal;
|
||||
} else
|
||||
{
|
||||
btVector3 torqueAxis0 = rel_pos1.cross(contactNormal);
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos1CrossNormal = torqueAxis0;
|
||||
solverConstraint.m_contactNormal1 = contactNormal;
|
||||
solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
|
||||
}
|
||||
|
||||
|
||||
|
||||
if (multiBodyB)
|
||||
{
|
||||
if (solverConstraint.m_linkB<0)
|
||||
{
|
||||
rel_pos2 = pos2 - multiBodyB->getBasePos();
|
||||
} else
|
||||
{
|
||||
rel_pos2 = pos2 - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
|
||||
}
|
||||
|
||||
const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
|
||||
|
||||
solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
|
||||
@@ -344,12 +373,18 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
|
||||
multiBodyB->calcAccelerationDeltasMultiDof(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
||||
else
|
||||
multiBodyB->calcAccelerationDeltas(&m_data.m_jacobians[solverConstraint.m_jacBindex],&m_data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex],m_data.scratch_r, m_data.scratch_v);
|
||||
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -contactNormal;
|
||||
|
||||
} else
|
||||
{
|
||||
btVector3 torqueAxis1 = rel_pos2.cross(contactNormal);
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
|
||||
solverConstraint.m_contactNormal2 = -contactNormal;
|
||||
|
||||
solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
|
||||
}
|
||||
|
||||
{
|
||||
@@ -577,6 +612,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
|
||||
{
|
||||
BT_PROFILE("addMultiBodyFrictionConstraint");
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
|
||||
solverConstraint.m_useJointForce = false;
|
||||
solverConstraint.m_frictionIndex = frictionIndex;
|
||||
bool isFriction = true;
|
||||
|
||||
@@ -644,6 +680,7 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
|
||||
int frictionIndex = m_multiBodyNormalContactConstraints.size();
|
||||
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints.expandNonInitializing();
|
||||
solverConstraint.m_useJointForce = false;
|
||||
|
||||
// btRigidBody* rb0 = btRigidBody::upcast(colObj0);
|
||||
// btRigidBody* rb1 = btRigidBody::upcast(colObj1);
|
||||
@@ -824,32 +861,267 @@ void btMultiBodyConstraintSolver::convertContacts(btPersistentManifold** manifol
|
||||
btScalar btMultiBodyConstraintSolver::solveGroup(btCollisionObject** bodies,int numBodies,btPersistentManifold** manifold,int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& info, btIDebugDraw* debugDrawer,btDispatcher* dispatcher)
|
||||
{
|
||||
return btSequentialImpulseConstraintSolver::solveGroup(bodies,numBodies,manifold,numManifolds,constraints,numConstraints,info,debugDrawer,dispatcher);
|
||||
}
|
||||
|
||||
#if 0
|
||||
static void applyJointFeedback(btMultiBodyJacobianData& data, const btMultiBodySolverConstraint& solverConstraint, int jacIndex, btMultiBody* mb, btScalar appliedImpulse)
|
||||
{
|
||||
if (appliedImpulse!=0 && mb->internalNeedsJointFeedback())
|
||||
{
|
||||
//todo: get rid of those temporary memory allocations for the joint feedback
|
||||
btAlignedObjectArray<btScalar> forceVector;
|
||||
int numDofsPlusBase = 6+mb->getNumDofs();
|
||||
forceVector.resize(numDofsPlusBase);
|
||||
for (int i=0;i<numDofsPlusBase;i++)
|
||||
{
|
||||
forceVector[i] = data.m_jacobians[jacIndex+i]*appliedImpulse;
|
||||
}
|
||||
btAlignedObjectArray<btScalar> output;
|
||||
output.resize(numDofsPlusBase);
|
||||
bool applyJointFeedback = true;
|
||||
mb->calcAccelerationDeltasMultiDof(&forceVector[0],&output[0],data.scratch_r,data.scratch_v,applyJointFeedback);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
#include "Bullet3Common/b3Logging.h"
|
||||
void btMultiBodyConstraintSolver::writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& c, btScalar deltaTime)
|
||||
{
|
||||
#if 1
|
||||
|
||||
//bod->addBaseForce(m_gravity * bod->getBaseMass());
|
||||
//bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
||||
|
||||
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
btScalar ai = c.m_appliedImpulse;
|
||||
|
||||
if(c.m_multiBodyA->isMultiDof())
|
||||
{
|
||||
if (c.m_useJointForce)
|
||||
{
|
||||
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||
//c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
|
||||
} else
|
||||
//if (c.m_multiBodyA->getCompanionId()>=0)
|
||||
{
|
||||
c.m_multiBodyA->setCompanionId(-1);
|
||||
btVector3 force = c.m_contactNormal1*(c.m_appliedImpulse/deltaTime);
|
||||
btVector3 torque = c.m_relpos1CrossNormal*(c.m_appliedImpulse/deltaTime);
|
||||
if (c.m_linkA<0)
|
||||
{
|
||||
c.m_multiBodyA->addBaseForce(force);
|
||||
c.m_multiBodyA->addBaseTorque(torque);
|
||||
} else
|
||||
{
|
||||
if (c.m_useJointForce)
|
||||
{
|
||||
c.m_multiBodyA->addJointTorqueMultiDof(c.m_linkA,0,c.m_appliedImpulse/deltaTime);
|
||||
} else
|
||||
{
|
||||
c.m_multiBodyA->addLinkForce(c.m_linkA,force);
|
||||
//b3Printf("force = %f,%f,%f\n",force[0],force[1],force[2]);//[0],torque[1],torque[2]);
|
||||
c.m_multiBodyA->addLinkTorque(c.m_linkA,torque);
|
||||
}
|
||||
}
|
||||
//c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
if(c.m_multiBodyB->isMultiDof())
|
||||
{
|
||||
if (c.m_useJointForce)
|
||||
{
|
||||
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||
} else
|
||||
|
||||
//if (c.m_multiBodyB->getCompanionId()>=0)
|
||||
{
|
||||
c.m_multiBodyB->setCompanionId(-1);
|
||||
btVector3 force = c.m_contactNormal2*(c.m_appliedImpulse/deltaTime);
|
||||
btVector3 torque = c.m_relpos2CrossNormal*(c.m_appliedImpulse/deltaTime);
|
||||
if (c.m_linkB<0)
|
||||
{
|
||||
c.m_multiBodyB->addBaseForce(force);
|
||||
c.m_multiBodyB->addBaseTorque(torque);
|
||||
} else
|
||||
{
|
||||
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]);
|
||||
c.m_multiBodyB->addLinkTorque(c.m_linkB,torque);
|
||||
}
|
||||
|
||||
}
|
||||
//c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
#else
|
||||
|
||||
if (c.m_multiBodyA)
|
||||
{
|
||||
|
||||
if(c.m_multiBodyA->isMultiDof())
|
||||
{
|
||||
c.m_multiBodyA->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||
}
|
||||
else
|
||||
{
|
||||
c.m_multiBodyA->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacAindex],c.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
|
||||
if (c.m_multiBodyB)
|
||||
{
|
||||
if(c.m_multiBodyB->isMultiDof())
|
||||
{
|
||||
c.m_multiBodyB->applyDeltaVeeMultiDof(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||
}
|
||||
else
|
||||
{
|
||||
c.m_multiBodyB->applyDeltaVee(&m_data.m_deltaVelocitiesUnitImpulse[c.m_jacBindex],c.m_appliedImpulse);
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionObject** bodies,int numBodies,const btContactSolverInfo& infoGlobal)
|
||||
{
|
||||
BT_PROFILE("btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish");
|
||||
int numPoolConstraints = m_multiBodyNormalContactConstraints.size();
|
||||
int j;
|
||||
|
||||
#ifndef DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
|
||||
//write back the delta v to the multi bodies, either as applied impulse (direct velocity change)
|
||||
//or as applied force, so we can measure the joint reaction forces easier
|
||||
for (int i=0;i<numPoolConstraints;i++)
|
||||
{
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i];
|
||||
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
|
||||
|
||||
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep);
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep);
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
|
||||
{
|
||||
btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
|
||||
writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep);
|
||||
}
|
||||
#endif //DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
|
||||
|
||||
if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
|
||||
{
|
||||
for (j=0;j<numPoolConstraints;j++)
|
||||
BT_PROFILE("warm starting write back");
|
||||
for (int j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btMultiBodySolverConstraint& solveManifold = m_multiBodyNormalContactConstraints[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solveManifold.m_originalContactPoint;
|
||||
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
|
||||
btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint;
|
||||
btAssert(pt);
|
||||
pt->m_appliedImpulse = solveManifold.m_appliedImpulse;
|
||||
|
||||
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex].m_appliedImpulse;
|
||||
pt->m_appliedImpulse = solverConstraint.m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral1 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse;
|
||||
|
||||
//printf("pt->m_appliedImpulseLateral1 = %f\n", pt->m_appliedImpulseLateral1);
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solveManifold.m_frictionIndex+1].m_appliedImpulse;
|
||||
pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse;
|
||||
}
|
||||
//do a callback here?
|
||||
}
|
||||
}
|
||||
#if 0
|
||||
//multibody joint feedback
|
||||
{
|
||||
BT_PROFILE("multi body joint feedback");
|
||||
for (int j=0;j<numPoolConstraints;j++)
|
||||
{
|
||||
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[j];
|
||||
|
||||
//apply the joint feedback into all links of the btMultiBody
|
||||
//todo: double-check the signs of the applied impulse
|
||||
|
||||
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||
}
|
||||
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
|
||||
}
|
||||
#if 0
|
||||
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacAindex,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyA,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||
|
||||
}
|
||||
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_jacBindex,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_multiBodyB,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
|
||||
}
|
||||
|
||||
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
|
||||
{
|
||||
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacAindex,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyA,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||
}
|
||||
|
||||
if (m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB && m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_jacBindex,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_multiBodyB,
|
||||
m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse*btSimdScalar(-1./infoGlobal.m_timeStep));
|
||||
}
|
||||
}
|
||||
#endif
|
||||
}
|
||||
|
||||
for (int i=0;i<m_multiBodyNonContactConstraints.size();i++)
|
||||
{
|
||||
const btMultiBodySolverConstraint& solverConstraint = m_multiBodyNonContactConstraints[i];
|
||||
if(solverConstraint.m_multiBodyA && solverConstraint.m_multiBodyA->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacAindex,solverConstraint.m_multiBodyA, solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||
}
|
||||
if(solverConstraint.m_multiBodyB && solverConstraint.m_multiBodyB->isMultiDof())
|
||||
{
|
||||
applyJointFeedback(m_data,solverConstraint, solverConstraint.m_jacBindex,solverConstraint.m_multiBodyB,solverConstraint.m_appliedImpulse*btSimdScalar(1./infoGlobal.m_timeStep));
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
numPoolConstraints = m_multiBodyNonContactConstraints.size();
|
||||
|
||||
@@ -878,7 +1150,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO
|
||||
|
||||
}
|
||||
#endif
|
||||
|
||||
#endif
|
||||
|
||||
return btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinish(bodies,numBodies,infoGlobal);
|
||||
}
|
||||
|
||||
@@ -19,6 +19,7 @@ subject to the following restrictions:
|
||||
#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
|
||||
#include "btMultiBodySolverConstraint.h"
|
||||
|
||||
//#define DIRECTLY_UPDATE_VELOCITY_DURING_SOLVER_ITERATIONS
|
||||
|
||||
class btMultiBody;
|
||||
|
||||
@@ -66,7 +67,7 @@ protected:
|
||||
|
||||
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies ,int numBodies,btPersistentManifold** manifoldPtr, int numManifolds,btTypedConstraint** constraints,int numConstraints,const btContactSolverInfo& infoGlobal,btIDebugDraw* debugDrawer);
|
||||
void applyDeltaVee(btScalar* deltaV, btScalar impulse, int velocityIndex, int ndof);
|
||||
|
||||
void writeBackSolverBodyToMultiBody(btMultiBodySolverConstraint& constraint, btScalar deltaTime);
|
||||
public:
|
||||
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
@@ -394,8 +394,20 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld ()
|
||||
delete m_solverMultiBodyIslandCallback;
|
||||
}
|
||||
|
||||
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];
|
||||
bod->forwardKinematics(world_to_local,local_origin);
|
||||
}
|
||||
}
|
||||
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
forwardKinematics();
|
||||
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
@@ -431,7 +443,7 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
|
||||
|
||||
{
|
||||
BT_PROFILE("btMultiBody addForce and stepVelocities");
|
||||
BT_PROFILE("btMultiBody addForce");
|
||||
for (int i=0;i<this->m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* bod = m_multiBodies[i];
|
||||
@@ -461,7 +473,36 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
||||
}
|
||||
}//if (!isSleeping)
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
{
|
||||
BT_PROFILE("btMultiBody stepVelocities");
|
||||
for (int i=0;i<this->m_multiBodies.size();i++)
|
||||
{
|
||||
btMultiBody* bod = m_multiBodies[i];
|
||||
|
||||
bool isSleeping = false;
|
||||
|
||||
if (bod->getBaseCollider() && bod->getBaseCollider()->getActivationState() == ISLAND_SLEEPING)
|
||||
{
|
||||
isSleeping = true;
|
||||
}
|
||||
for (int b=0;b<bod->getNumLinks();b++)
|
||||
{
|
||||
if (bod->getLink(b).m_collider && bod->getLink(b).m_collider->getActivationState()==ISLAND_SLEEPING)
|
||||
isSleeping = true;
|
||||
}
|
||||
|
||||
if (!isSleeping)
|
||||
{
|
||||
//useless? they get resized in stepVelocities once again (AND DIFFERENTLY)
|
||||
scratch_r.resize(bod->getNumLinks()+1); //multidof? ("Y"s use it and it is used to store qdd)
|
||||
scratch_v.resize(bod->getNumLinks()+1);
|
||||
scratch_m.resize(bod->getNumLinks()+1);
|
||||
bool doNotUpdatePos = false;
|
||||
|
||||
if(bod->isMultiDof())
|
||||
@@ -639,6 +680,8 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
|
||||
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
|
||||
|
||||
|
||||
|
||||
}
|
||||
|
||||
void btMultiBodyDynamicsWorld::integrateTransforms(btScalar timeStep)
|
||||
@@ -779,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++)
|
||||
{
|
||||
@@ -791,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);
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@@ -38,7 +38,7 @@ protected:
|
||||
virtual void calculateSimulationIslands();
|
||||
virtual void updateActivationState(btScalar timeStep);
|
||||
virtual void solveConstraints(btContactSolverInfo& solverInfo);
|
||||
|
||||
|
||||
public:
|
||||
|
||||
btMultiBodyDynamicsWorld(btDispatcher* dispatcher,btBroadphaseInterface* pairCache,btMultiBodyConstraintSolver* constraintSolver,btCollisionConfiguration* collisionConfiguration);
|
||||
@@ -49,6 +49,16 @@ public:
|
||||
|
||||
virtual void removeMultiBody(btMultiBody* body);
|
||||
|
||||
virtual int getNumMultibodies() const
|
||||
{
|
||||
return m_multiBodies.size();
|
||||
}
|
||||
|
||||
btMultiBody* getMultiBody(int mbIndex)
|
||||
{
|
||||
return m_multiBodies[mbIndex];
|
||||
}
|
||||
|
||||
virtual void addMultiBodyConstraint( btMultiBodyConstraint* constraint);
|
||||
|
||||
virtual int getNumMultiBodyConstraints() const
|
||||
@@ -73,5 +83,8 @@ public:
|
||||
virtual void debugDrawWorld();
|
||||
|
||||
virtual void debugDrawMultiBodyConstraint(btMultiBodyConstraint* constraint);
|
||||
|
||||
void forwardKinematics();
|
||||
|
||||
};
|
||||
#endif //BT_MULTIBODY_DYNAMICS_WORLD_H
|
||||
|
||||
28
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
Normal file
28
src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h
Normal file
@@ -0,0 +1,28 @@
|
||||
/*
|
||||
Copyright (c) 2015 Google Inc.
|
||||
|
||||
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.
|
||||
*/
|
||||
|
||||
|
||||
|
||||
#ifndef BT_MULTIBODY_JOINT_FEEDBACK_H
|
||||
#define BT_MULTIBODY_JOINT_FEEDBACK_H
|
||||
|
||||
#include "LinearMath/btSpatialAlgebra.h"
|
||||
|
||||
struct btMultiBodyJointFeedback
|
||||
{
|
||||
btSpatialForceVector m_reactionForces;
|
||||
btSymmetricSpatialDyad m_spatialInertia;
|
||||
};
|
||||
|
||||
#endif //BT_MULTIBODY_JOINT_FEEDBACK_H
|
||||
@@ -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,6 +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);
|
||||
|
||||
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;
|
||||
|
||||
@@ -22,21 +22,21 @@ 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
|
||||
|
||||
|
||||
}
|
||||
|
||||
void btMultiBodyJointMotor::finalizeMultiDof()
|
||||
{
|
||||
allocateJacobiansMultiDof();
|
||||
|
||||
void btMultiBodyJointMotor::finalizeMultiDof()
|
||||
{
|
||||
allocateJacobiansMultiDof();
|
||||
// note: we rely on the fact that data.m_jacobians are
|
||||
// always initialized to zero by the Constraint ctor
|
||||
int linkDoF = 0;
|
||||
@@ -44,14 +44,14 @@ void btMultiBodyJointMotor::finalizeMultiDof()
|
||||
|
||||
// row 0: the lower bound
|
||||
// row 0: the lower bound
|
||||
jacobianA(0)[offset] = 1;
|
||||
|
||||
m_numDofsFinalized = m_jacSizeBoth;
|
||||
jacobianA(0)[offset] = 1;
|
||||
|
||||
m_numDofsFinalized = m_jacSizeBoth;
|
||||
}
|
||||
|
||||
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,14 +98,14 @@ 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();
|
||||
}
|
||||
|
||||
//don't crash
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
{
|
||||
finalizeMultiDof();
|
||||
}
|
||||
|
||||
//don't crash
|
||||
if (m_numDofsFinalized != m_jacSizeBoth)
|
||||
return;
|
||||
|
||||
const btScalar posError = 0;
|
||||
@@ -117,6 +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);
|
||||
|
||||
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);
|
||||
}
|
||||
};
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@@ -33,7 +33,7 @@ public:
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
btMultiBodyJointMotor(btMultiBody* body, int link, int linkDoF, btScalar desiredVelocity, btScalar maxMotorImpulse);
|
||||
virtual ~btMultiBodyJointMotor();
|
||||
virtual void finalizeMultiDof();
|
||||
virtual void finalizeMultiDof();
|
||||
|
||||
virtual int getIslandIdA() const;
|
||||
virtual int getIslandIdB() const;
|
||||
|
||||
@@ -25,6 +25,7 @@ enum btMultiBodyLinkFlags
|
||||
BT_MULTIBODYLINKFLAGS_DISABLE_PARENT_COLLISION = 1
|
||||
};
|
||||
|
||||
//both defines are now permanently enabled
|
||||
#define BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
#define TEST_SPATIAL_ALGEBRA_LAYER
|
||||
|
||||
@@ -34,11 +35,9 @@ enum btMultiBodyLinkFlags
|
||||
|
||||
//namespace {
|
||||
|
||||
#ifdef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
|
||||
#include "LinearMath/btSpatialAlgebra.h"
|
||||
|
||||
#endif
|
||||
//}
|
||||
|
||||
//
|
||||
@@ -64,18 +63,14 @@ 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
|
||||
{
|
||||
eRevolute = 0,
|
||||
ePrismatic = 1,
|
||||
eSpherical = 2,
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
ePlanar = 3,
|
||||
#endif
|
||||
eFixed = 4,
|
||||
eInvalid
|
||||
};
|
||||
@@ -95,16 +90,6 @@ struct btMultibodyLink
|
||||
// m_axesTop[1][2] = zero
|
||||
// m_axesBottom[0] = zero
|
||||
// m_axesBottom[1][2] = unit vectors along the translational axes on that plane
|
||||
#ifndef TEST_SPATIAL_ALGEBRA_LAYER
|
||||
btVector3 m_axesTop[6];
|
||||
btVector3 m_axesBottom[6];
|
||||
void setAxisTop(int dof, const btVector3 &axis) { m_axesTop[dof] = axis; }
|
||||
void setAxisBottom(int dof, const btVector3 &axis) { m_axesBottom[dof] = axis; }
|
||||
void setAxisTop(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesTop[dof].setValue(x, y, z); }
|
||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axesBottom[dof].setValue(x, y, z); }
|
||||
const btVector3 & getAxisTop(int dof) const { return m_axesTop[dof]; }
|
||||
const btVector3 & getAxisBottom(int dof) const { return m_axesBottom[dof]; }
|
||||
#else
|
||||
btSpatialMotionVector m_axes[6];
|
||||
void setAxisTop(int dof, const btVector3 &axis) { m_axes[dof].m_topVec = axis; }
|
||||
void setAxisBottom(int dof, const btVector3 &axis) { m_axes[dof].m_bottomVec = axis; }
|
||||
@@ -112,7 +97,6 @@ struct btMultibodyLink
|
||||
void setAxisBottom(int dof, const btScalar &x, const btScalar &y, const btScalar &z) { m_axes[dof].m_bottomVec.setValue(x, y, z); }
|
||||
const btVector3 & getAxisTop(int dof) const { return m_axes[dof].m_topVec; }
|
||||
const btVector3 & getAxisBottom(int dof) const { return m_axes[dof].m_bottomVec; }
|
||||
#endif
|
||||
|
||||
int m_dofOffset, m_cfgOffset;
|
||||
|
||||
@@ -120,17 +104,25 @@ struct btMultibodyLink
|
||||
btVector3 m_cachedRVector; // 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
|
||||
btVector3 m_appliedTorque; // In WORLD frame
|
||||
|
||||
btScalar m_jointPos[7];
|
||||
btScalar m_jointTorque[6]; //TODO
|
||||
|
||||
|
||||
//m_jointTorque is the joint torque applied by the user using 'addJointTorque'.
|
||||
//It gets set to zero after each internal stepSimulation call
|
||||
btScalar m_jointTorque[6];
|
||||
|
||||
class btMultiBodyLinkCollider* m_collider;
|
||||
int m_flags;
|
||||
|
||||
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
|
||||
eFeatherstoneJointType m_jointType;
|
||||
|
||||
struct btMultiBodyJointFeedback* m_jointFeedback;
|
||||
|
||||
btTransform m_cachedWorldTransform;//this cache is updated when calling btMultiBody::forwardKinematics
|
||||
|
||||
// ctor: set some sensible defaults
|
||||
btMultibodyLink()
|
||||
@@ -142,8 +134,11 @@ struct btMultibodyLink
|
||||
m_flags(0),
|
||||
m_dofCount(0),
|
||||
m_posVarCount(0),
|
||||
m_jointType(btMultibodyLink::eInvalid)
|
||||
m_jointType(btMultibodyLink::eInvalid),
|
||||
m_jointFeedback(0)
|
||||
|
||||
{
|
||||
|
||||
m_inertiaLocal.setValue(1, 1, 1);
|
||||
setAxisTop(0, 0., 0., 0.);
|
||||
setAxisBottom(0, 1., 0., 0.);
|
||||
@@ -156,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
|
||||
@@ -200,7 +196,6 @@ struct btMultibodyLink
|
||||
|
||||
break;
|
||||
}
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case ePlanar:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
|
||||
@@ -208,7 +203,6 @@ struct btMultibodyLink
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
case eFixed:
|
||||
{
|
||||
m_cachedRotParentToThis = m_zeroRotParentToThis;
|
||||
|
||||
@@ -43,11 +43,11 @@ btMultiBodyPoint2Point::btMultiBodyPoint2Point(btMultiBody* bodyA, int linkA, bt
|
||||
m_pivotInB(pivotInB)
|
||||
{
|
||||
}
|
||||
|
||||
void btMultiBodyPoint2Point::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
|
||||
void btMultiBodyPoint2Point::finalizeMultiDof()
|
||||
{
|
||||
//not implemented yet
|
||||
btAssert(0);
|
||||
}
|
||||
|
||||
btMultiBodyPoint2Point::~btMultiBodyPoint2Point()
|
||||
@@ -108,6 +108,7 @@ int numDim = BTMBP2PCONSTRAINT_DIM;
|
||||
|
||||
btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
|
||||
//memset(&constraintRow,0xffffffff,sizeof(btMultiBodySolverConstraint));
|
||||
constraintRow.m_useJointForce = false;
|
||||
constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
|
||||
constraintRow.m_contactNormal1.setValue(0,0,0);
|
||||
constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
|
||||
|
||||
@@ -28,7 +28,7 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
||||
{
|
||||
BT_DECLARE_ALIGNED_ALLOCATOR();
|
||||
|
||||
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1)
|
||||
btMultiBodySolverConstraint() : m_solverBodyIdA(-1), m_multiBodyA(0), m_linkA(-1), m_solverBodyIdB(-1), m_multiBodyB(0), m_linkB(-1),m_useJointForce(false)
|
||||
{}
|
||||
|
||||
int m_deltaVelAindex;//more generic version of m_relpos1CrossNormal/m_contactNormal1
|
||||
@@ -73,6 +73,8 @@ ATTRIBUTE_ALIGNED16 (struct) btMultiBodySolverConstraint
|
||||
btMultiBody* m_multiBodyB;
|
||||
int m_linkB;
|
||||
|
||||
bool m_useJointForce;//needed for write-back of joint versus link/base force/torque
|
||||
|
||||
enum btSolverConstraintType
|
||||
{
|
||||
BT_SOLVER_CONTACT_1D = 0,
|
||||
|
||||
Reference in New Issue
Block a user