Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

expose pybullet 'enableConeFriction' to switch between pyramid and cone friction model.

Browse Source
This commit is contained in:
Erwin Coumans
2017-12-01 10:07:07 -08:00
parent e1df1534f3
commit c556ad651d
7 changed files with 83 additions and 51 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -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);
}

9
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -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)
{

3
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -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);

1
examples/pybullet/examples/frictionCone.py
View File

@@ -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)

11
examples/pybullet/pybullet.c
View File

@@ -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);
}

94
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp
View File

@@ -69,20 +69,43 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
}
//solve featherstone frictional contact
if (infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS && ((infoGlobal.m_solverMode&SOLVER_DISABLE_IMPLICIT_CONE_FRICTION) == 0))
{
for (int j1 = 0; j1<this->m_multiBodyTorsionalFrictionContactConstraints.size(); j1++)
{
if (iteration < infoGlobal.m_numIterations)
{
int index = j1;//iteration&1? j1 : m_multiBodyTorsionalFrictionContactConstraints.size()-1-j1;
for (int j1=0;j1<this->m_multiBodyLateralFrictionContactConstraints.size();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_multiBodyLateralFrictionContactConstraints[index];
btMultiBodySolverConstraint& frictionConstraint = m_multiBodyFrictionContactConstraints[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))
{
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
{
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;
//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;j1<this->m_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?
}

2
src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h
View File

@@ -35,7 +35,7 @@ protected:
btMultiBodyConstraintArray m_multiBodyNonContactConstraints;
btMultiBodyConstraintArray m_multiBodyNormalContactConstraints;
btMultiBodyConstraintArray m_multiBodyLateralFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyFrictionContactConstraints;
btMultiBodyConstraintArray m_multiBodyTorsionalFrictionContactConstraints;
btMultiBodyJacobianData m_data;