mirror of
https://github.com/bulletphysics/bullet3
synced 2025-01-18 21:10:05 +00:00
First step in btMultiBody joint force/torque feedback. There is still some work to be done for 'mobilizer limit/motor'.
See examples/MultiBody/TestJointTorqueSetup and examples/Constraints/TestHingeTorque for joint feedback.
This commit is contained in:
erwincoumans
parent
41aa58560b
commit
89edc40d61
@ -4,12 +4,15 @@
|
||||
#include "../CommonInterfaces/CommonRigidBodyBase.h"
|
||||
#include "../CommonInterfaces/CommonParameterInterface.h"
|
||||
|
||||
short collisionFilterGroup = short(btBroadphaseProxy::CharacterFilter);
|
||||
short collisionFilterMask = short(btBroadphaseProxy::AllFilter ^ (btBroadphaseProxy::StaticFilter|btBroadphaseProxy::CharacterFilter));
|
||||
|
||||
|
||||
struct TestHingeTorque : public CommonRigidBodyBase
|
||||
{
|
||||
bool m_once;
|
||||
|
||||
btAlignedObjectArray<btJointFeedback*> m_jointFeedback;
|
||||
|
||||
TestHingeTorque(struct GUIHelperInterface* helper);
|
||||
virtual ~ TestHingeTorque();
|
||||
virtual void initPhysics();
|
||||
@ -37,6 +40,11 @@ m_once(true)
|
||||
}
|
||||
TestHingeTorque::~ TestHingeTorque()
|
||||
{
|
||||
for (int i=0;i<m_jointFeedback.size();i++)
|
||||
{
|
||||
delete m_jointFeedback[i];
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -70,6 +78,22 @@ void TestHingeTorque::stepSimulation(float deltaTime)
|
||||
|
||||
child->getAngularVelocity()[2]);
|
||||
|
||||
for (int i=0;i<m_jointFeedback.size();i++)
|
||||
{
|
||||
b3Printf("Applied force A:(%f,%f,%f), torque A:(%f,%f,%f)\nForce B:(%f,%f,%f), torque B:(%f,%f,%f)\n",
|
||||
m_jointFeedback[i]->m_appliedForceBodyA.x(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyA.y(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyA.z(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyA.x(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyA.y(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyA.z(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.x(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.y(),
|
||||
m_jointFeedback[i]->m_appliedForceBodyB.z(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.x(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.y(),
|
||||
m_jointFeedback[i]->m_appliedTorqueBodyB.z());
|
||||
}
|
||||
//CommonRigidBodyBase::stepSimulation(deltaTime);
|
||||
}
|
||||
|
||||
@ -77,10 +101,10 @@ void TestHingeTorque::stepSimulation(float deltaTime)
|
||||
|
||||
void TestHingeTorque::initPhysics()
|
||||
{
|
||||
m_guiHelper->setUpAxis(2);
|
||||
m_guiHelper->setUpAxis(1);
|
||||
|
||||
createEmptyDynamicsWorld();
|
||||
m_dynamicsWorld->setGravity(btVector3(0,0,0));
|
||||
// m_dynamicsWorld->setGravity(btVector3(0,0,0));
|
||||
|
||||
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
|
||||
int mode = btIDebugDraw::DBG_DrawWireframe
|
||||
@ -107,13 +131,13 @@ void TestHingeTorque::initPhysics()
|
||||
//mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm
|
||||
//init the base
|
||||
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
|
||||
float baseMass = 1.f;
|
||||
float baseMass = 0.f;
|
||||
float linkMass = 1.f;
|
||||
|
||||
btRigidBody* base = createRigidBody(baseMass,baseWorldTrans,baseBox);
|
||||
m_dynamicsWorld->removeRigidBody(base);
|
||||
base->setDamping(0,0);
|
||||
m_dynamicsWorld->addRigidBody(base,0,0);
|
||||
m_dynamicsWorld->addRigidBody(base,collisionFilterGroup,collisionFilterMask);
|
||||
btBoxShape* linkBox = new btBoxShape(linkHalfExtents);
|
||||
btRigidBody* prevBody = base;
|
||||
|
||||
@ -126,7 +150,7 @@ void TestHingeTorque::initPhysics()
|
||||
|
||||
btRigidBody* linkBody = createRigidBody(linkMass,linkTrans,linkBox);
|
||||
m_dynamicsWorld->removeRigidBody(linkBody);
|
||||
m_dynamicsWorld->addRigidBody(linkBody,0,0);
|
||||
m_dynamicsWorld->addRigidBody(linkBody,collisionFilterGroup,collisionFilterMask);
|
||||
linkBody->setDamping(0,0);
|
||||
//create a hinge constraint
|
||||
btVector3 pivotInA(0,-linkHalfExtents[1],0);
|
||||
@ -137,6 +161,10 @@ void TestHingeTorque::initPhysics()
|
||||
btHingeConstraint* hinge = new btHingeConstraint(*prevBody,*linkBody,
|
||||
pivotInA,pivotInB,
|
||||
axisInA,axisInB,useReferenceA);
|
||||
btJointFeedback* fb = new btJointFeedback();
|
||||
m_jointFeedback.push_back(fb);
|
||||
hinge->setJointFeedback(fb);
|
||||
|
||||
m_dynamicsWorld->addConstraint(hinge,true);
|
||||
prevBody = linkBody;
|
||||
|
||||
|
||||
@ -2,12 +2,17 @@
|
||||
#include "TestJointTorqueSetup.h"
|
||||
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
|
||||
|
||||
#include "BulletDynamics/Featherstone/btMultiBodyJointFeedback.h"
|
||||
|
||||
#include "../CommonInterfaces/CommonMultiBodyBase.h"
|
||||
|
||||
|
||||
|
||||
struct TestJointTorqueSetup : public CommonMultiBodyBase
|
||||
{
|
||||
btMultiBody* m_multiBody;
|
||||
btAlignedObjectArray<btMultiBodyJointFeedback*> m_jointFeedbacks;
|
||||
|
||||
bool m_once;
|
||||
public:
|
||||
|
||||
@ -43,7 +48,7 @@ TestJointTorqueSetup::~TestJointTorqueSetup()
|
||||
|
||||
void TestJointTorqueSetup::initPhysics()
|
||||
{
|
||||
int upAxis = 2;
|
||||
int upAxis = 1;
|
||||
m_guiHelper->setUpAxis(upAxis);
|
||||
|
||||
btVector4 colors[4] =
|
||||
@ -68,8 +73,10 @@ void TestJointTorqueSetup::initPhysics()
|
||||
+btIDebugDraw::DBG_DrawAabb
|
||||
);//+btIDebugDraw::DBG_DrawConstraintLimits);
|
||||
|
||||
|
||||
|
||||
//create a static ground object
|
||||
if (0)
|
||||
if (1)
|
||||
{
|
||||
btVector3 groundHalfExtents(20,20,20);
|
||||
groundHalfExtents[upAxis]=1.f;
|
||||
@ -89,14 +96,14 @@ void TestJointTorqueSetup::initPhysics()
|
||||
}
|
||||
|
||||
{
|
||||
bool floating = true;
|
||||
bool damping = false;
|
||||
bool floating = false;
|
||||
bool damping = true;
|
||||
bool gyro = false;
|
||||
int numLinks = 1;
|
||||
bool spherical = false; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals
|
||||
bool canSleep = false;
|
||||
bool selfCollide = false;
|
||||
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
|
||||
btVector3 linkHalfExtents(0.05, 0.37, 0.1);
|
||||
btVector3 baseHalfExtents(0.05, 0.37, 0.1);
|
||||
|
||||
btVector3 basePosition = btVector3(-0.4f, 3.f, 0.f);
|
||||
@ -107,15 +114,18 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
if(baseMass)
|
||||
{
|
||||
btCollisionShape *pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
|
||||
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
|
||||
delete pTempBox;
|
||||
//btCollisionShape *shape = new btSphereShape(baseHalfExtents[0]);// btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
|
||||
btCollisionShape *shape = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
|
||||
shape->calculateLocalInertia(baseMass, baseInertiaDiag);
|
||||
delete shape;
|
||||
}
|
||||
|
||||
bool isMultiDof = false;
|
||||
bool isMultiDof = true;
|
||||
btMultiBody *pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, !floating, canSleep, isMultiDof);
|
||||
|
||||
m_multiBody = pMultiBody;
|
||||
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
|
||||
// baseOriQuat.setEulerZYX(-.25*SIMD_PI,0,-1.75*SIMD_PI);
|
||||
pMultiBody->setBasePos(basePosition);
|
||||
pMultiBody->setWorldToBaseRot(baseOriQuat);
|
||||
btVector3 vel(0, 0, 0);
|
||||
@ -123,13 +133,7 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
//init the links
|
||||
btVector3 hingeJointAxis(1, 0, 0);
|
||||
float linkMass = 1.f;
|
||||
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
|
||||
|
||||
btCollisionShape *pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
|
||||
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
|
||||
delete pTempBox;
|
||||
|
||||
|
||||
//y-axis assumed up
|
||||
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
|
||||
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
|
||||
@ -143,15 +147,46 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
for(int i = 0; i < numLinks; ++i)
|
||||
{
|
||||
float linkMass = 1.f;
|
||||
//if (i==3 || i==2)
|
||||
// linkMass= 1000;
|
||||
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
|
||||
|
||||
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));//new btSphereShape(linkHalfExtents[0]);
|
||||
shape->calculateLocalInertia(linkMass, linkInertiaDiag);
|
||||
delete shape;
|
||||
|
||||
|
||||
if(!spherical)
|
||||
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||
{
|
||||
//pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||
|
||||
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1,
|
||||
btQuaternion(0.f, 0.f, 0.f, 1.f),
|
||||
hingeJointAxis,
|
||||
parentComToCurrentPivot,
|
||||
currentPivotToCurrentCom, false);
|
||||
|
||||
//pMultiBody->setupFixed(i,linkMass,linkInertiaDiag,i-1,btQuaternion(0,0,0,1),parentComToCurrentPivot,currentPivotToCurrentCom,false);
|
||||
|
||||
}
|
||||
else
|
||||
{
|
||||
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
|
||||
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, false);
|
||||
}
|
||||
}
|
||||
|
||||
//pMultiBody->finalizeMultiDof();
|
||||
pMultiBody->finalizeMultiDof();
|
||||
|
||||
//for (int i=pMultiBody->getNumLinks()-1;i>=0;i--)//
|
||||
for (int i=0;i<pMultiBody->getNumLinks();i++)
|
||||
{
|
||||
btMultiBodyJointFeedback* fb = new btMultiBodyJointFeedback();
|
||||
pMultiBody->getLink(i).m_jointFeedback = fb;
|
||||
m_jointFeedbacks.push_back(fb);
|
||||
//break;
|
||||
}
|
||||
btMultiBodyDynamicsWorld* world = m_dynamicsWorld;
|
||||
|
||||
///
|
||||
@ -171,7 +206,8 @@ void TestJointTorqueSetup::initPhysics()
|
||||
}
|
||||
//
|
||||
btVector3 gravity(0,0,0);
|
||||
//gravity[upAxis] = -9.81;
|
||||
gravity[upAxis] = -9.81;
|
||||
gravity[0] = 0;
|
||||
m_dynamicsWorld->setGravity(gravity);
|
||||
//////////////////////////////////////////////
|
||||
if(0)//numLinks > 0)
|
||||
@ -207,11 +243,11 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
if (1)
|
||||
{
|
||||
btCollisionShape* box = new btBoxShape(baseHalfExtents);
|
||||
m_guiHelper->createCollisionShapeGraphicsObject(box);
|
||||
btCollisionShape* shape = new btBoxShape(btVector3(baseHalfExtents[0],baseHalfExtents[1],baseHalfExtents[2]));//new btSphereShape(baseHalfExtents[0]);
|
||||
m_guiHelper->createCollisionShapeGraphicsObject(shape);
|
||||
|
||||
btMultiBodyLinkCollider* col= new btMultiBodyLinkCollider(pMultiBody, -1);
|
||||
col->setCollisionShape(box);
|
||||
col->setCollisionShape(shape);
|
||||
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
@ -222,7 +258,12 @@ void TestJointTorqueSetup::initPhysics()
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
|
||||
world->addCollisionObject(col, 2,1+2);
|
||||
bool isDynamic = (baseMass > 0 && floating);
|
||||
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
|
||||
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//, 2,1+2);
|
||||
|
||||
btVector3 color(0.0,0.0,0.5);
|
||||
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
|
||||
@ -250,25 +291,33 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
float quat[4]={-world_to_local[i+1].x(),-world_to_local[i+1].y(),-world_to_local[i+1].z(),world_to_local[i+1].w()};
|
||||
|
||||
btCollisionShape* box = new btBoxShape(linkHalfExtents);
|
||||
m_guiHelper->createCollisionShapeGraphicsObject(box);
|
||||
btCollisionShape* shape = new btBoxShape(btVector3(linkHalfExtents[0],linkHalfExtents[1],linkHalfExtents[2]));//btSphereShape(linkHalfExtents[0]);
|
||||
m_guiHelper->createCollisionShapeGraphicsObject(shape);
|
||||
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
|
||||
|
||||
col->setCollisionShape(box);
|
||||
col->setCollisionShape(shape);
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(posr);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
col->setWorldTransform(tr);
|
||||
col->setFriction(friction);
|
||||
world->addCollisionObject(col,2,1+2);
|
||||
btVector4 color = colors[curColor];
|
||||
bool isDynamic = 1;//(linkMass > 0);
|
||||
short collisionFilterGroup = isDynamic? short(btBroadphaseProxy::DefaultFilter) : short(btBroadphaseProxy::StaticFilter);
|
||||
short collisionFilterMask = isDynamic? short(btBroadphaseProxy::AllFilter) : short(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
|
||||
|
||||
//if (i==0||i>numLinks-2)
|
||||
{
|
||||
world->addCollisionObject(col,collisionFilterGroup,collisionFilterMask);//,2,1+2);
|
||||
btVector4 color = colors[curColor];
|
||||
curColor++;
|
||||
curColor&=3;
|
||||
m_guiHelper->createCollisionObjectGraphicsObject(col,color);
|
||||
|
||||
|
||||
pMultiBody->getLink(i).m_collider=col;
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
|
||||
@ -276,7 +325,8 @@ void TestJointTorqueSetup::initPhysics()
|
||||
|
||||
void TestJointTorqueSetup::stepSimulation(float deltaTime)
|
||||
{
|
||||
if (m_once)
|
||||
//m_multiBody->addLinkForce(0,btVector3(100,100,100));
|
||||
if (0)//m_once)
|
||||
{
|
||||
m_once=false;
|
||||
m_multiBody->addJointTorque(0, 10.0);
|
||||
@ -286,14 +336,34 @@ void TestJointTorqueSetup::stepSimulation(float deltaTime)
|
||||
}
|
||||
|
||||
m_dynamicsWorld->stepSimulation(1./60,0);
|
||||
|
||||
if (1)
|
||||
for (int i=0;i<m_jointFeedbacks.size();i++)
|
||||
{
|
||||
b3Printf("F_reaction[%i] linear:%f,%f,%f, angular:%f,%f,%f",
|
||||
i,
|
||||
m_jointFeedbacks[i]->m_reactionForces.m_topVec[0],
|
||||
m_jointFeedbacks[i]->m_reactionForces.m_topVec[1],
|
||||
m_jointFeedbacks[i]->m_reactionForces.m_topVec[2],
|
||||
|
||||
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[0],
|
||||
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[1],
|
||||
m_jointFeedbacks[i]->m_reactionForces.m_bottomVec[2]
|
||||
|
||||
);
|
||||
|
||||
}
|
||||
|
||||
|
||||
/*
|
||||
b3Printf("base angvel = %f,%f,%f",m_multiBody->getBaseOmega()[0],
|
||||
m_multiBody->getBaseOmega()[1],
|
||||
m_multiBody->getBaseOmega()[2]
|
||||
);
|
||||
|
||||
*/
|
||||
btScalar jointVel =m_multiBody->getJointVel(0);
|
||||
|
||||
b3Printf("child angvel = %f",jointVel);
|
||||
// b3Printf("child angvel = %f",jointVel);
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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,188 +1547,6 @@ 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
|
||||
{
|
||||
@ -2303,7 +1707,6 @@ void btMultiBody::calcAccelerationDeltasMultiDof(const btScalar *force, btScalar
|
||||
//printf("]\n");
|
||||
/////////////////
|
||||
}
|
||||
#endif
|
||||
|
||||
|
||||
void btMultiBody::calcAccelerationDeltas(const btScalar *force, btScalar *output,
|
||||
@ -2595,7 +1998,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 +2009,6 @@ void btMultiBody::stepPositionsMultiDof(btScalar dt, btScalar *pq, btScalar *pqd
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
{
|
||||
}
|
||||
@ -2722,7 +2123,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 +2131,6 @@ void btMultiBody::filConstraintJacobianMultiDof(int link,
|
||||
|
||||
break;
|
||||
}
|
||||
#endif
|
||||
default:
|
||||
{
|
||||
}
|
||||
@ -2894,3 +2293,6 @@ void btMultiBody::checkMotionAndSleepIfRequired(btScalar timestep)
|
||||
wakeUp();
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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,14 @@ public:
|
||||
setWorldToBaseRot(tr.getRotation().inverse());
|
||||
|
||||
}
|
||||
|
||||
btTransform getBaseWorldTransform() const
|
||||
{
|
||||
btTransform tr;
|
||||
tr.setOrigin(getBasePos());
|
||||
tr.setRotation(getWorldToBaseRot().inverse());
|
||||
}
|
||||
|
||||
void setBaseVel(const btVector3 &vel)
|
||||
{
|
||||
|
||||
@ -217,6 +224,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 +398,8 @@ public:
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
|
||||
// timestep the positions (given current velocities).
|
||||
void stepPositions(btScalar dt);
|
||||
void stepPositionsMultiDof(btScalar dt, btScalar *pq = 0, btScalar *pqd = 0);
|
||||
@ -536,6 +547,12 @@ public:
|
||||
__posUpdated = updated;
|
||||
}
|
||||
|
||||
//internalNeedsJointFeedback is for internal use only
|
||||
bool internalNeedsJointFeedback() const
|
||||
{
|
||||
return m_internalNeedsJointFeedback;
|
||||
}
|
||||
|
||||
private:
|
||||
btMultiBody(const btMultiBody &); // not implemented
|
||||
void operator=(const btMultiBody &); // not implemented
|
||||
@ -543,9 +560,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 +574,7 @@ private:
|
||||
|
||||
void mulMatrix(btScalar *pA, btScalar *pB, int rowsA, int colsA, int rowsB, int colsB, btScalar *pC) const;
|
||||
|
||||
|
||||
|
||||
private:
|
||||
|
||||
@ -622,6 +638,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_worldPosition;
|
||||
}
|
||||
|
||||
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_worldPosition;
|
||||
}
|
||||
|
||||
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_worldPosition;
|
||||
}
|
||||
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_worldPosition;
|
||||
}
|
||||
|
||||
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,50 @@ 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];
|
||||
int nLinks = bod->getNumLinks();
|
||||
///base + num m_links
|
||||
world_to_local.resize(nLinks+1);
|
||||
local_origin.resize(nLinks+1);
|
||||
|
||||
world_to_local[0] = bod->getWorldToBaseRot();
|
||||
local_origin[0] = bod->getBasePos();
|
||||
|
||||
for (int k=0;k<bod->getNumLinks();k++)
|
||||
{
|
||||
const int parent = bod->getParent(k);
|
||||
world_to_local[k+1] = bod->getParentToLocalRot(k) * world_to_local[parent+1];
|
||||
local_origin[k+1] = local_origin[parent+1] + (quatRotate(world_to_local[k+1].inverse() , bod->getRVector(k)));
|
||||
}
|
||||
|
||||
for (int link=0;link<bod->getNumLinks();link++)
|
||||
{
|
||||
int index = link+1;
|
||||
|
||||
btVector3 posr = local_origin[index];
|
||||
btScalar quat[4]={-world_to_local[index].x(),-world_to_local[index].y(),-world_to_local[index].z(),world_to_local[index].w()};
|
||||
btTransform tr;
|
||||
tr.setIdentity();
|
||||
tr.setOrigin(posr);
|
||||
tr.setRotation(btQuaternion(quat[0],quat[1],quat[2],quat[3]));
|
||||
bod->getLink(link).m_worldPosition = posr;
|
||||
|
||||
}
|
||||
|
||||
}
|
||||
}
|
||||
void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
forwardKinematics();
|
||||
|
||||
btAlignedObjectArray<btScalar> scratch_r;
|
||||
btAlignedObjectArray<btVector3> scratch_v;
|
||||
@ -431,7 +473,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 +503,39 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
{
|
||||
bod->addLinkForce(j, m_gravity * bod->getLinkMass(j));
|
||||
}
|
||||
}//if (!isSleeping)
|
||||
}
|
||||
}
|
||||
|
||||
m_solverMultiBodyIslandCallback->processConstraints();
|
||||
|
||||
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
|
||||
|
||||
|
||||
{
|
||||
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())
|
||||
@ -635,9 +709,6 @@ void btMultiBodyDynamicsWorld::solveConstraints(btContactSolverInfo& solverInfo)
|
||||
}
|
||||
}
|
||||
|
||||
m_solverMultiBodyIslandCallback->processConstraints();
|
||||
|
||||
m_constraintSolver->allSolved(solverInfo, m_debugDrawer);
|
||||
|
||||
}
|
||||
|
||||
|
||||
@ -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,7 @@ 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
|
||||
@ -116,6 +116,7 @@ void btMultiBodyJointLimitConstraint::createConstraintRows(btMultiBodyConstraint
|
||||
const btVector3 dummy(0, 0, 0);
|
||||
|
||||
btScalar rel_vel = fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,0,m_maxAppliedImpulse);
|
||||
constraintRow.m_useJointForce = true;
|
||||
{
|
||||
btScalar penetration = getPosition(row);
|
||||
btScalar positionalError = 0.f;
|
||||
|
||||
@ -33,10 +33,10 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal
|
||||
|
||||
|
||||
}
|
||||
|
||||
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,9 +44,9 @@ 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)
|
||||
@ -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,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con
|
||||
|
||||
|
||||
fillMultiBodyConstraint(constraintRow,data,jacobianA(row),jacobianB(row),dummy,dummy,dummy,posError,infoGlobal,-m_maxAppliedImpulse,m_maxAppliedImpulse,1,false,m_desiredVelocity);
|
||||
constraintRow.m_useJointForce = true;
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -133,7 +117,12 @@ struct btMultibodyLink
|
||||
|
||||
|
||||
int m_dofCount, m_posVarCount; //redundant but handy
|
||||
|
||||
eFeatherstoneJointType m_jointType;
|
||||
|
||||
struct btMultiBodyJointFeedback* m_jointFeedback;
|
||||
|
||||
btVector3 m_worldPosition;
|
||||
|
||||
// ctor: set some sensible defaults
|
||||
btMultibodyLink()
|
||||
@ -145,7 +134,9 @@ struct btMultibodyLink
|
||||
m_flags(0),
|
||||
m_dofCount(0),
|
||||
m_posVarCount(0),
|
||||
m_jointType(btMultibodyLink::eInvalid)
|
||||
m_jointType(btMultibodyLink::eInvalid),
|
||||
m_jointFeedback(0),
|
||||
m_worldPosition(0,0,0)
|
||||
{
|
||||
m_inertiaLocal.setValue(1, 1, 1);
|
||||
setAxisTop(0, 0., 0., 0.);
|
||||
@ -203,7 +194,6 @@ struct btMultibodyLink
|
||||
|
||||
break;
|
||||
}
|
||||
#ifdef BT_MULTIBODYLINK_INCLUDE_PLANAR_JOINTS
|
||||
case ePlanar:
|
||||
{
|
||||
m_cachedRotParentToThis = btQuaternion(getAxisTop(0),-pJointPos[0]) * m_zeroRotParentToThis;
|
||||
@ -211,7 +201,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,
|
||||
|
||||
Loading…
Reference in New Issue
Block a user