diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index b84cb60a6..c3711f20f 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -124,7 +124,7 @@ int main(int argc, char* argv[]) b3Vector3 basePos; b3Quaternion baseOrn; sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn); - sim->resetDebugVisualizerCamera(distance,yaw,20,basePos); + sim->resetDebugVisualizerCamera(distance,-20, yaw,basePos); } b3Clock::usleep(1000.*1000.*fixedTimeStep); } diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 36e5d4330..e75213c94 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -450,6 +450,15 @@ B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCo } +B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + command->m_physSimParamArgs.m_enableConeFriction = enableConeFriction; + command->m_updateFlags |= SIM_PARAM_ENABLE_CONE_FRICTION; + return 0; +} + B3_SHARED_API int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 63bc958cf..fa450608d 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -280,6 +280,9 @@ B3_SHARED_API int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryComman B3_SHARED_API int b3PhysicsParamSetMaxNumCommandsPer1ms(b3SharedMemoryCommandHandle commandHandle, int maxNumCmdPer1ms); B3_SHARED_API int b3PhysicsParamSetEnableFileCaching(b3SharedMemoryCommandHandle commandHandle, int enableFileCaching); B3_SHARED_API int b3PhysicsParamSetRestitutionVelocityThreshold(b3SharedMemoryCommandHandle commandHandle, double restitutionVelocityThreshold); +B3_SHARED_API int b3PhysicsParamSetEnableConeFriction(b3SharedMemoryCommandHandle commandHandle, int enableConeFriction); + + B3_SHARED_API b3SharedMemoryCommandHandle b3InitRequestPhysicsParamCommand(b3PhysicsClientHandle physClient); diff --git a/examples/pybullet/examples/frictionCone.py b/examples/pybullet/examples/frictionCone.py index 88e4b966d..96e9883a9 100644 --- a/examples/pybullet/examples/frictionCone.py +++ b/examples/pybullet/examples/frictionCone.py @@ -16,6 +16,7 @@ num = 40 p.configureDebugVisualizer(p.COV_ENABLE_GUI,0) p.configureDebugVisualizer(p.COV_ENABLE_RENDERING,1)#disable this to make it faster p.configureDebugVisualizer(p.COV_ENABLE_TINY_RENDERER,0) +p.setPhysicsEngineParameter(enableConeFriction=1) for i in range (num): print("progress:",i,num) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index bde55bce6..b96911674 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -925,13 +925,14 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double erp = -1; double contactERP = -1; double frictionERP = -1; + int enableConeFriction = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "physicsClientId", NULL}; + static char* kwlist[] = {"fixedTimeStep", "numSolverIterations", "useSplitImpulse", "splitImpulsePenetrationThreshold", "numSubSteps", "collisionFilterMode", "contactBreakingThreshold", "maxNumCmdPer1ms", "enableFileCaching","restitutionVelocityThreshold", "erp", "contactERP", "frictionERP", "enableConeFriction", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddi", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, - &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidiiddddii", kwlist, &fixedTimeStep, &numSolverIterations, &useSplitImpulse, &splitImpulsePenetrationThreshold, &numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &maxNumCmdPer1ms, &enableFileCaching, &restitutionVelocityThreshold, &erp, &contactERP, &frictionERP, &enableConeFriction, &physicsClientId)) { return NULL; } @@ -1003,6 +1004,10 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetDefaultFrictionERP(command,frictionERP); } + if (enableConeFriction >= 0) + { + b3PhysicsParamSetEnableConeFriction(command, enableConeFriction); + } statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 3f3a73153..7ed088c9f 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -67,22 +67,45 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl if(constraint.m_multiBodyB) constraint.m_multiBodyB->setPosUpdated(false); } - - //solve featherstone frictional contact - for (int j1=0;j1m_multiBodyLateralFrictionContactConstraints.size();j1++) + //solve featherstone frictional contact + if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)) { - if (iteration < infoGlobal.m_numIterations) + for (int j1 = 0; j1m_multiBodyTorsionalFrictionContactConstraints.size(); j1++) { - int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyLateralFrictionContactConstraints[index]; - - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION)==0)) + if (iteration < infoGlobal.m_numIterations) { + int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; + //adjust friction limits here + if (totalImpulse>btScalar(0)) + { + frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); + frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; + btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); + leastSquaredResidual += residual*residual; + + if (frictionConstraint.m_multiBodyA) + frictionConstraint.m_multiBodyA->setPosUpdated(false); + if (frictionConstraint.m_multiBodyB) + frictionConstraint.m_multiBodyB->setPosUpdated(false); + } + } + } + + for (int j1 = 0; j1 < this->m_multiBodyFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) + { + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; + + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; j1++; int index2 = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyLateralFrictionContactConstraints[index2]; + btMultiBodySolverConstraint& frictionConstraintB = m_multiBodyFrictionContactConstraints[index2]; btAssert(frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex); if (frictionConstraint.m_frictionIndex == frictionConstraintB.m_frictionIndex) @@ -91,17 +114,29 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; frictionConstraintB.m_lowerLimit = -(frictionConstraintB.m_friction*totalImpulse); frictionConstraintB.m_upperLimit = frictionConstraintB.m_friction*totalImpulse; - leastSquaredResidual += resolveConeFrictionConstraintRows(frictionConstraint,frictionConstraintB); - if(frictionConstraint.m_multiBodyA) + leastSquaredResidual += resolveConeFrictionConstraintRows(frictionConstraint, frictionConstraintB); + if (frictionConstraint.m_multiBodyA) frictionConstraint.m_multiBodyA->setPosUpdated(false); - if(frictionConstraint.m_multiBodyB) + if (frictionConstraint.m_multiBodyB) frictionConstraint.m_multiBodyB->setPosUpdated(false); } } - else + } + + + } + else + { + for (int j1 = 0; j1m_multiBodyFrictionContactConstraints.size(); j1++) + { + if (iteration < infoGlobal.m_numIterations) { + int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; + + btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[index]; + btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; //adjust friction limits here - if (totalImpulse > btScalar(0)) + if (totalImpulse>btScalar(0)) { frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; @@ -116,30 +151,6 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl } } } - - for (int j1=0;j1m_multiBodyTorsionalFrictionContactConstraints.size();j1++) - { - if (iteration < infoGlobal.m_numIterations) - { - int index = j1;//iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1; - - btMultiBodySolverConstraint& frictionConstraint = m_multiBodyTorsionalFrictionContactConstraints[index]; - btScalar totalImpulse = m_multiBodyNormalContactConstraints[frictionConstraint.m_frictionIndex].m_appliedImpulse; - //adjust friction limits here - if (totalImpulse>btScalar(0)) - { - frictionConstraint.m_lowerLimit = -(frictionConstraint.m_friction*totalImpulse); - frictionConstraint.m_upperLimit = frictionConstraint.m_friction*totalImpulse; - btScalar residual = resolveSingleConstraintRowGeneric(frictionConstraint); - leastSquaredResidual += residual*residual; - - if(frictionConstraint.m_multiBodyA) - frictionConstraint.m_multiBodyA->setPosUpdated(false); - if(frictionConstraint.m_multiBodyB) - frictionConstraint.m_multiBodyB->setPosUpdated(false); - } - } - } return leastSquaredResidual; } @@ -147,7 +158,7 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlySetup(btCollisionOb { m_multiBodyNonContactConstraints.resize(0); m_multiBodyNormalContactConstraints.resize(0); - m_multiBodyLateralFrictionContactConstraints.resize(0); + m_multiBodyFrictionContactConstraints.resize(0); m_multiBodyTorsionalFrictionContactConstraints.resize(0); m_data.m_jacobians.resize(0); @@ -1103,7 +1114,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMu btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionConstraint(const btVector3& normalAxis,btPersistentManifold* manifold,int frictionIndex,btManifoldPoint& cp,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyLateralFrictionContactConstraints.expandNonInitializing(); + btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); solverConstraint.m_orgConstraint = 0; solverConstraint.m_orgDofIndex = -1; @@ -1140,7 +1151,10 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyTorsionalF btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip) { BT_PROFILE("addMultiBodyRollingFrictionConstraint"); - btMultiBodySolverConstraint& solverConstraint = m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing(); + + bool useTorsionalAndConeFriction = (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0)); + + btMultiBodySolverConstraint& solverConstraint = useTorsionalAndConeFriction? m_multiBodyTorsionalFrictionContactConstraints.expandNonInitializing() : m_multiBodyFrictionContactConstraints.expandNonInitializing(); solverConstraint.m_orgConstraint = 0; solverConstraint.m_orgDofIndex = -1; @@ -1507,11 +1521,11 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO btMultiBodySolverConstraint& solverConstraint = m_multiBodyNormalContactConstraints[i]; writeBackSolverBodyToMultiBody(solverConstraint,infoGlobal.m_timeStep); - writeBackSolverBodyToMultiBody(m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex],infoGlobal.m_timeStep); if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS)) { - writeBackSolverBodyToMultiBody(m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep); + writeBackSolverBodyToMultiBody(m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1],infoGlobal.m_timeStep); } } @@ -1532,12 +1546,12 @@ btScalar btMultiBodyConstraintSolver::solveGroupCacheFriendlyFinish(btCollisionO btManifoldPoint* pt = (btManifoldPoint*) solverConstraint.m_originalContactPoint; btAssert(pt); pt->m_appliedImpulse = solverConstraint.m_appliedImpulse; - pt->m_appliedImpulseLateral1 = m_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex].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_multiBodyLateralFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; + pt->m_appliedImpulseLateral2 = m_multiBodyFrictionContactConstraints[solverConstraint.m_frictionIndex+1].m_appliedImpulse; } //do a callback here? } diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h index 99b344e29..6361465cd 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h @@ -35,7 +35,7 @@ protected: btMultiBodyConstraintArray m_multiBodyNonContactConstraints; btMultiBodyConstraintArray m_multiBodyNormalContactConstraints; - btMultiBodyConstraintArray m_multiBodyLateralFrictionContactConstraints; + btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints; btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints; btMultiBodyJacobianData m_data;