diff --git a/examples/Constraints/TestHingeTorque.cpp b/examples/Constraints/TestHingeTorque.cpp index 4842e7f45..8afec175c 100644 --- a/examples/Constraints/TestHingeTorque.cpp +++ b/examples/Constraints/TestHingeTorque.cpp @@ -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 m_jointFeedback; + TestHingeTorque(struct GUIHelperInterface* helper); virtual ~ TestHingeTorque(); virtual void initPhysics(); @@ -37,6 +40,11 @@ m_once(true) } TestHingeTorque::~ TestHingeTorque() { + for (int i=0;igetAngularVelocity()[2]); + for (int i=0;im_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; diff --git a/examples/MultiBody/TestJointTorqueSetup.cpp b/examples/MultiBody/TestJointTorqueSetup.cpp index 2927904e9..bd5b0e10a 100644 --- a/examples/MultiBody/TestJointTorqueSetup.cpp +++ b/examples/MultiBody/TestJointTorqueSetup.cpp @@ -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 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;igetNumLinks();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;im_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); diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 5b65864ae..31d0c719a 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -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 &scratch_r, btAlignedObjectArray &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 &scratch_r, - btAlignedObjectArray &scratch_v, - btAlignedObjectArray &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 &scratch_r, btAlignedObjectArray &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(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 &scratch_r, btAlignedObjectArray &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(); } } + + + diff --git a/src/BulletDynamics/Featherstone/btMultiBody.h b/src/BulletDynamics/Featherstone/btMultiBody.h index c24b6db2d..b0c924d37 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.h +++ b/src/BulletDynamics/Featherstone/btMultiBody.h @@ -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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp index bafb841a3..dbc801268 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraint.cpp @@ -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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index d1bdd084c..71512f961 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -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 forceVector; + int numDofsPlusBase = 6+mb->getNumDofs(); + forceVector.resize(numDofsPlusBase); + for (int i=0;i 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;im_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;jisMultiDof()) + { + 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;iisMultiDof()) + { + 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); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index f11a007aa..10f899fa2 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -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(); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp index 891bd3099..b5096c97e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.cpp @@ -394,8 +394,50 @@ btMultiBodyDynamicsWorld::~btMultiBodyDynamicsWorld () delete m_solverMultiBodyIslandCallback; } +void btMultiBodyDynamicsWorld::forwardKinematics() +{ + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + + + + for (int b=0;bgetNumLinks(); + ///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;kgetNumLinks();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;linkgetNumLinks();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 scratch_r; btAlignedObjectArray 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;im_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;im_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;bgetNumLinks();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); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h index dd0f7711f..633e427f8 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h @@ -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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h b/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h new file mode 100644 index 000000000..fe2ef15e8 --- /dev/null +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointFeedback.h @@ -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 diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp index c984d41e0..6bc6f77de 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.cpp @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index b08509567..8cc1ef3c3 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -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; } } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index e946a69b8..d322f87e7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyLink.h b/src/BulletDynamics/Featherstone/btMultiBodyLink.h index 93538e594..7ec774b20 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyLink.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyLink.h @@ -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; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp index 7d029690b..4e5911e00 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyPoint2Point.cpp @@ -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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h index 7bc64569a..5a8580913 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h +++ b/src/BulletDynamics/Featherstone/btMultiBodySolverConstraint.h @@ -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,