diff --git a/data/cube_gripper_left.urdf b/data/cube_gripper_left.urdf index f46f69bcf..26c5389af 100644 --- a/data/cube_gripper_left.urdf +++ b/data/cube_gripper_left.urdf @@ -6,10 +6,8 @@ - + - - diff --git a/data/cube_gripper_right.urdf b/data/cube_gripper_right.urdf index 68a202175..ca642dbfe 100644 --- a/data/cube_gripper_right.urdf +++ b/data/cube_gripper_right.urdf @@ -6,10 +6,8 @@ - + - - diff --git a/data/cube_small.urdf b/data/cube_small.urdf index d28667c8e..0bf03f5aa 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,7 +3,7 @@ - + diff --git a/data/plane.urdf b/data/plane.urdf index fb7c124af..57b746104 100644 --- a/data/plane.urdf +++ b/data/plane.urdf @@ -1,9 +1,6 @@ - - - diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index b9fed9e53..6f9a5b496 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -33,6 +33,7 @@ + diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h index 14d19ec3d..ddb9ceb8f 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -46,7 +46,7 @@ public: virtual bool mouseButtonCallback(int button, int state, float x, float y)=0; virtual bool keyboardCallback(int key, int state)=0; - virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]) {} + virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis) {} virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){} }; diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 6d2cfd3e0..0639eea64 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -27,6 +27,8 @@ #include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "SharedMemoryCommands.h" +btVector3 gLastPickPos(0, 0, 0); + struct UrdfLinkNameMapUtil { btMultiBody* m_mb; @@ -383,7 +385,7 @@ struct PhysicsServerCommandProcessorInternalData btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; btMultiBody* m_gripperMultiBody; - + int m_huskyId; CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -435,6 +437,7 @@ struct PhysicsServerCommandProcessorInternalData m_gripperRigidbodyFixed(0), m_gripperMultiBody(0), m_allowRealTimeSimulation(false), + m_huskyId(0), m_commandLogger(0), m_logPlayback(0), m_physicsDeltaTime(1./240.), @@ -1874,10 +1877,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { applyJointDamping(i); } - if (m_data->m_numSimulationSubSteps > 0) - m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,m_data->m_numSimulationSubSteps,m_data->m_physicsDeltaTime/m_data->m_numSimulationSubSteps); - else - m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,0); + + if (m_data->m_numSimulationSubSteps > 0) + { + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, m_data->m_numSimulationSubSteps, m_data->m_physicsDeltaTime / m_data->m_numSimulationSubSteps); + } + else + { + m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime, 0); + } SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; @@ -2678,7 +2686,7 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags) } } -btVector3 gLastPickPos(0,0,0); + bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld) { @@ -2832,6 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) btVector3 gVRGripperPos(0,0,0); btQuaternion gVRGripperOrn(0,0,0,1); +btScalar gVRGripperAnalog = 0; bool gVRGripperClosed = false; @@ -2860,7 +2869,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { int bodyId = 0; - if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) + if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 0.1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) { InteralBodyData* parentBody = m_data->getHandle(bodyId); if (parentBody->m_multiBody) @@ -2878,14 +2887,25 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) m_data->m_gripperMultiBody = parentBody->m_multiBody; if (m_data->m_gripperMultiBody->getNumLinks() > 2) { - m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI); - m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI); + m_data->m_gripperMultiBody->setJointPos(0, 0); + m_data->m_gripperMultiBody->setJointPos(2, 0); } m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); } } + + loadUrdf("sphere2.urdf", btVector3(-2, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("sphere2.urdf", btVector3(-2, 0, 2), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("sphere2.urdf", btVector3(-2, 0, 3), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + + loadUrdf("samurai.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("kuka_iiwa/model.urdf", btVector3(3, 0, 0), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("cube_small.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + loadUrdf("husky/husky.urdf", btVector3(1, 1, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); + m_data->m_huskyId = bodyId; + } } @@ -2904,14 +2924,9 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) { motor->setErp(0.01); - if (gVRGripperClosed) - { - motor->setPositionTarget(0.2, 1); - } - else - { - motor->setPositionTarget(SIMD_HALF_PI, 1); - } + motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1); + + motor->setVelocityTarget(0, 0.1); btScalar maxImp = 1550.*m_data->m_physicsDeltaTime; motor->setMaxAppliedImpulse(maxImp); diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 9622c5aa4..837bb5c21 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -18,6 +18,8 @@ extern btVector3 gLastPickPos; btVector3 gVRTeleportPos(0,0,0); +btQuaternion gVRTeleportOrn(0, 0, 0,1); + extern bool gVRGripperClosed; bool gDebugRenderToggle = false; @@ -556,7 +558,7 @@ public: btVector3 getRayTo(int x,int y); virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]); - virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4]); + virtual void vrControllerMoveCallback(int controllerId, float pos[4], float orientation[4], float analogAxis); virtual bool mouseMoveCallback(float x,float y) { @@ -894,6 +896,7 @@ void PhysicsServerExample::renderScene() ///debug rendering //m_args[0].m_cs->lock(); + //gVRTeleportPos[0] += 0.01; vrOffset[12]=-gVRTeleportPos[0]; vrOffset[13]=-gVRTeleportPos[1]; vrOffset[14]=-gVRTeleportPos[2]; @@ -1101,7 +1104,7 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt } -static int gGraspingController = 2; +static int gGraspingController = -1; void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]) { @@ -1110,6 +1113,8 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS) return; + if (gGraspingController < 0) + gGraspingController = controllerId; if (controllerId != gGraspingController) { @@ -1151,10 +1156,15 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt extern btVector3 gVRGripperPos; extern btQuaternion gVRGripperOrn; +extern btScalar gVRGripperAnalog; -void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4]) + + +void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[4], float orn[4], float analogAxis) { + + if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS) { printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); @@ -1162,6 +1172,7 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ } if (controllerId == gGraspingController) { + gVRGripperAnalog = analogAxis; gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]); gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI); diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 26f1a88f1..32941de76 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -713,14 +713,17 @@ bool CMainApplication::HandleInput() } else { + // printf("Device MOVED: %d\n", unDevice); - sExample->vrControllerMoveCallback(unDevice, pos, orn); + sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x); } } else { if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller ) { + + b3Transform tr; getControllerTransform(unDevice, tr); float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] }; @@ -741,7 +744,7 @@ bool CMainApplication::HandleInput() } else { - sExample->vrControllerMoveCallback(unDevice, pos, orn); + sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x); } } } diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index a863a9492..96c1eb5d5 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -135,6 +135,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRestitution = calculateCombinedRestitution(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedRollingFriction = calculateCombinedRollingFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); + newPt.m_combinedSpinningFriction = calculateCombinedSpinningFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) || (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 57ea4e69e..aced8fae1 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -627,8 +627,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c } -void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, +void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, + btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) @@ -647,8 +647,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdB = solverBodyIdB; - solverConstraint.m_friction = cp.m_combinedRollingFriction; - solverConstraint.m_originalContactPoint = 0; + solverConstraint.m_friction = combinedTorsionalFriction; + solverConstraint.m_originalContactPoint = 0; solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedPushImpulse = 0.f; @@ -704,11 +704,11 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv -btSolverConstraint& btSequentialImpulseConstraintSolver::addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) +btSolverConstraint& btSequentialImpulseConstraintSolver::addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip) { btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing(); solverConstraint.m_frictionIndex = frictionIndex; - setupRollingFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2, + setupTorsionalFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, combinedTorsionalFriction,rel_pos1, rel_pos2, colObj0, colObj1, relaxation, desiredVelocity, cfmSlip); return solverConstraint; } @@ -1069,28 +1069,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); - btVector3 angVelA(0,0,0),angVelB(0,0,0); - if (rb0) - angVelA = rb0->getAngularVelocity(); - if (rb1) - angVelB = rb1->getAngularVelocity(); - btVector3 relAngVel = angVelB-angVelA; - if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0)) { - //only a single rollingFriction per manifold - //rollingFriction--; - if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold) + { - relAngVel.normalize(); - applyAnisotropicFriction(colObj0,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - applyAnisotropicFriction(colObj1,relAngVel,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); - if (relAngVel.length()>0.001) - addRollingFrictionConstraint(relAngVel,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); - - } else - { - addRollingFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); btVector3 axis0,axis1; btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); axis0.normalize(); @@ -1101,9 +1084,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); if (axis0.length()>0.001) - addRollingFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addTorsionalFrictionConstraint(axis0,solverBodyIdA,solverBodyIdB,frictionIndex,cp, + cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); if (axis1.length()>0.001) - addRollingFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation); + addTorsionalFrictionConstraint(axis1,solverBodyIdA,solverBodyIdB,frictionIndex,cp, + cp.m_combinedRollingFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation); } } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index a60291809..7eafa3a68 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -54,13 +54,13 @@ protected: btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, - btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, + void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, + btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0., btScalar cfmSlip=0.); - btSolverConstraint& addRollingFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); + btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,int frictionIndex,btManifoldPoint& cp,btScalar torsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity=0, btScalar cfmSlip=0.f); void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index a70e82d70..f2ab1b1ee 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -557,9 +557,11 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol } -void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, +void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& constraintNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) { @@ -784,7 +786,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMult } } - solverConstraint.m_friction = cp.m_combinedRollingFriction; + solverConstraint.m_friction =combinedTorsionalFriction; if(!isFriction) { @@ -860,7 +862,9 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo return solverConstraint; } -btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) +btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyRollingFrictionConstraint"); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); @@ -891,7 +895,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyRollingFri solverConstraint.m_originalContactPoint = &cp; - setupMultiBodyRollingFrictionConstraint(solverConstraint, normalAxis, cp, infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); + setupMultiBodyTorsionalFrictionConstraint(solverConstraint, normalAxis, cp, combinedTorsionalFriction,infoGlobal,relaxation,isFriction, desiredVelocity, cfmSlip); return solverConstraint; } @@ -1010,9 +1014,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* if (rollingFriction > 0) { - addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - //addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - //addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); + addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal); rollingFriction--; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index ee5cda954..3a89c6373 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -50,7 +50,9 @@ protected: btMultiBodySolverConstraint& addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); - btMultiBodySolverConstraint& addMultiBodyRollingFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); + btMultiBodySolverConstraint& addMultiBodyTorsionalFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0, btScalar cfmSlip=0); void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow, btScalar* jacA,btScalar* jacB, @@ -63,9 +65,12 @@ protected: btScalar& relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); - void setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, + //either rolling or spinning friction + void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, const btVector3& contactNormal, - btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, + btManifoldPoint& cp, + btScalar combinedTorsionalFriction, + const btContactSolverInfo& infoGlobal, btScalar& relaxation, bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);