From 4094b9f0df2f09d1f10acff28fa5701ea850aaf2 Mon Sep 17 00:00:00 2001 From: erwin coumans Date: Sat, 27 Aug 2016 13:44:18 -0700 Subject: [PATCH] Disable 'm_robotSim.setNumSimulationSubSteps' because it is not needed at the moment Fix issue in contact/friction between btMultibody and btRigidBody (external force/torque of btRigidBody was not taken into account during contact/friction setup) Allow 0.1 mm slop in contact, to avoid loosing contact. Todo: allow contacts with positive distance in multibody solver. --- examples/RoboticsLearning/GripperGraspExample.cpp | 2 +- examples/SharedMemory/PhysicsServerCommandProcessor.cpp | 1 + .../Featherstone/btMultiBodyConstraintSolver.cpp | 8 ++++++-- 3 files changed, 8 insertions(+), 3 deletions(-) diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index a6086ada9..af7211f39 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -152,7 +152,7 @@ public: } m_robotSim.setGravity(b3MakeVector3(0,0,-10)); - m_robotSim.setNumSimulationSubSteps(4); + //m_robotSim.setNumSimulationSubSteps(4); } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 3b62bc52f..3c30f30df 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -517,6 +517,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() m_data = new PhysicsServerCommandProcessorInternalData(); createEmptyDynamicsWorld(); + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 08411f40b..a4636c3c0 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -408,7 +408,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { if (rb0) { - rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1); + rel_vel += (rb0->getVelocityInLocalPoint(rel_pos1) + + (rb0->getTotalTorque()*rb0->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos1)+ + rb0->getTotalForce()*rb0->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal1); } } if (multiBodyB) @@ -422,7 +424,9 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol { if (rb1) { - rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2); + rel_vel += (rb1->getVelocityInLocalPoint(rel_pos2)+ + (rb1->getTotalTorque()*rb1->getInvInertiaTensorWorld()*infoGlobal.m_timeStep).cross(rel_pos2) + + rb1->getTotalForce()*rb1->getInvMass()*infoGlobal.m_timeStep).dot(solverConstraint.m_contactNormal2); } }