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

fix issue with btMultiBody friction in combination with soft contacts (friction should not re-use normal contact cfm/erp)

Browse Source 
implement friction anchors, position friction correction, disabled by default. Use colObj->setCollisionFlag(flag | CF_HAS_FRICTION_ANCHOR); See test/RobotClientAPI/SlopeFrictionMain.cpp. In URDF or SDF, add <friction_anchor/> in <contact> section of <link> to enable.
PhysicsServer: properly restore old activation state after releasing picked object
btMultiBodyConstraintSolver: disable flip/flop of contact/friction constraint solving by default (it breaks some internal flaky unit tests)
This commit is contained in:
Erwin Coumans
2017-03-20 10:58:07 -07:00
parent 865d37fcb5
commit 0b017b0f53
18 changed files with 299 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

3
data/cube_soft.urdf
View File

@@ -2,7 +2,8 @@
<robot name="cube.urdf">
<link name="baseLink">
<contact>
<lateral_friction value="1.0"/>
<friction_anchor/>
<lateral_friction value="1.3"/>
<rolling_friction value="0.0"/>
<stiffness value="300"/>
<damping value="10"/>

4
examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
View File

@@ -166,6 +166,10 @@ void processContactParameters(const URDFLinkContactInfo& contactInfo, btCollisio
{
col->setContactStiffnessAndDamping(contactInfo.m_contactStiffness, contactInfo.m_contactDamping);
}
if ((contactInfo.m_flags & URDF_CONTACT_HAS_FRICTION_ANCHOR) != 0)
{
col->setCollisionFlags(col->getCollisionFlags() | btCollisionObject::CF_HAS_FRICTION_ANCHOR);
}
}

3
examples/Importers/ImportURDFDemo/URDFJointTypes.h
View File

@@ -23,7 +23,8 @@ enum URDF_LinkContactFlags
URDF_CONTACT_HAS_ROLLING_FRICTION=32,
URDF_CONTACT_HAS_SPINNING_FRICTION=64,
URDF_CONTACT_HAS_RESTITUTION=128,
URDF_CONTACT_HAS_FRICTION_ANCHOR=256,
};
struct URDFLinkContactInfo

7
examples/Importers/ImportURDFDemo/UrdfParser.cpp
View File

@@ -767,6 +767,13 @@ bool UrdfParser::parseLink(UrdfModel& model, UrdfLink& link, TiXmlElement *confi
}
}
}
{
TiXmlElement *friction_anchor = ci->FirstChildElement("friction_anchor");
if (friction_anchor)
{
link.m_contactInfo.m_flags |= URDF_CONTACT_HAS_FRICTION_ANCHOR;
}
}
{

33
examples/RobotSimulator/RobotSimulatorMain.cpp
View File

@@ -61,6 +61,7 @@ int main(int argc, char* argv[])
sim->setRealTimeSimulation(false);
int vidLogId = -1;
int minitaurLogId = -1;
int rotateCamera = 0;
while (sim->canSubmitCommand())
{
@@ -73,9 +74,11 @@ int main(int argc, char* argv[])
//m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased
for (int i=0;i<keyEvents.m_numKeyboardEvents;i++)
{
if (keyEvents.m_keyboardEvents[i].m_keyCode=='0')
b3KeyboardEvent& e = keyEvents.m_keyboardEvents[i];
if (e.m_keyCode=='0')
{
if ( keyEvents.m_keyboardEvents[i].m_keyState&eButtonTriggered)
if ( e.m_keyState&eButtonTriggered)
{
if (vidLogId < 0)
{
@@ -89,28 +92,40 @@ int main(int argc, char* argv[])
}
}
if (keyEvents.m_keyboardEvents[i].m_keyCode=='m')
if (e.m_keyCode=='m')
{
if ( minitaurLogId<0 && keyEvents.m_keyboardEvents[i].m_keyState&eButtonTriggered)
if ( minitaurLogId<0 && e.m_keyState&eButtonTriggered)
{
minitaurLogId = sim->startStateLogging(STATE_LOGGING_MINITAUR,"simlog.bin");
}
if (minitaurLogId>=0 && keyEvents.m_keyboardEvents[i].m_keyState&eButtonReleased)
if (minitaurLogId>=0 && e.m_keyState&eButtonReleased)
{
sim->stopStateLogging(minitaurLogId);
minitaurLogId=-1;
}
}
if (e.m_keyCode == 'r' && e.m_keyState&eButtonTriggered)
{
rotateCamera = 1-rotateCamera;
}
//printf("keyEvent[%d].m_keyCode = %d, state = %d\n", i,keyEvents.m_keyboardEvents[i].m_keyCode,keyEvents.m_keyboardEvents[i].m_keyState);
}
}
sim->stepSimulation();
static double yaw=0;
double distance = 10.5+9 * b3Sin(yaw);
yaw+=0.008;
sim->resetDebugVisualizerCamera(distance,yaw,20,b3MakeVector3(0,0,0.1));
if (rotateCamera)
{
static double yaw=0;
double distance = 1;
yaw+=0.1;
b3Vector3 basePos;
b3Quaternion baseOrn;
sim->getBasePositionAndOrientation(minitaurUid,basePos,baseOrn);
sim->resetDebugVisualizerCamera(distance,yaw,20,basePos);
}
b3Clock::usleep(1000.*1000.*fixedTimeStep);
}

4
examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
View File

@@ -210,7 +210,7 @@ void b3RobotSimulatorClientAPI::stepSimulation()
{
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
statusType = b3GetStatusType(statusHandle);
b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
//b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
}
}
@@ -226,7 +226,7 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
b3SharedMemoryStatusHandle statusHandle;
b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
// b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
}
b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)

4
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -914,7 +914,7 @@ b3SharedMemoryStatusHandle b3ProcessServerStatus(b3PhysicsClientHandle physClien
int b3GetStatusType(b3SharedMemoryStatusHandle statusHandle)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle;
b3Assert(status);
//b3Assert(status);
if (status)
{
return status->m_type;
@@ -1063,7 +1063,7 @@ b3SharedMemoryStatusHandle b3SubmitClientCommandAndWaitStatus(b3PhysicsClientHan
b3SubmitClientCommand(physClient, commandHandle);
while ((statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
while (cl->isConnected() && (statusHandle == 0) && (clock.getTimeInSeconds()-startTime < timeOutInSeconds))
{
statusHandle = b3ProcessServerStatus(physClient);
}

7
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -919,6 +919,7 @@ struct PhysicsServerCommandProcessorInternalData
//data for picking objects
class btRigidBody* m_pickedBody;
int m_savedActivationState;
class btTypedConstraint* m_pickedConstraint;
class btMultiBodyPoint2Point* m_pickingMultiBodyPoint2Point;
btVector3 m_oldPickingPos;
@@ -1146,7 +1147,8 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_dynamicsWorld->setGravity(btVector3(0, 0, 0));
m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08;
m_data->m_dynamicsWorld->getSolverInfo().m_frictionERP = 0.2;//need to check if there are artifacts with frictionERP
m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001;
m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7;
@@ -4978,6 +4980,7 @@ bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, cons
if (!(body->isStaticObject() || body->isKinematicObject()))
{
m_data->m_pickedBody = body;
m_data->m_savedActivationState = body->getActivationState();
m_data->m_pickedBody->setActivationState(DISABLE_DEACTIVATION);
//printf("pickPos=%f,%f,%f\n",pickPos.getX(),pickPos.getY(),pickPos.getZ());
btVector3 localPivot = body->getCenterOfMassTransform().inverse() * pickPos;
@@ -5070,7 +5073,7 @@ void PhysicsServerCommandProcessor::removePickingConstraint()
m_data->m_dynamicsWorld->removeConstraint(m_data->m_pickedConstraint);
delete m_data->m_pickedConstraint;
m_data->m_pickedConstraint = 0;
m_data->m_pickedBody->forceActivationState(ACTIVE_TAG);
m_data->m_pickedBody->forceActivationState(m_data->m_savedActivationState);
m_data->m_pickedBody = 0;
}
if (m_data->m_pickingMultiBodyPoint2Point)

1
src/BulletCollision/CollisionDispatch/btCollisionObject.h
View File

@@ -139,6 +139,7 @@ public:
CF_DISABLE_SPU_COLLISION_PROCESSING = 64,//disable parallel/SPU processing
CF_HAS_CONTACT_STIFFNESS_DAMPING = 128,
CF_HAS_CUSTOM_DEBUG_RENDERING_COLOR = 256,
CF_HAS_FRICTION_ANCHOR = 512,
};
enum CollisionObjectTypes

8
src/BulletCollision/CollisionDispatch/btManifoldResult.cpp
View File

@@ -145,7 +145,13 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedContactStiffness1 = calculateCombinedContactStiffness(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING;
}
if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_FRICTION_ANCHOR))
{
newPt.m_contactPointFlags |= BT_CONTACT_FLAG_FRICTION_ANCHOR;
}
btPlaneSpace1(newPt.m_normalWorldOnB,newPt.m_lateralFrictionDir1,newPt.m_lateralFrictionDir2);

1
src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h
View File

@@ -41,6 +41,7 @@ enum btContactPointFlags
BT_CONTACT_FLAG_HAS_CONTACT_CFM=2,
BT_CONTACT_FLAG_HAS_CONTACT_ERP=4,
BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING = 8,
BT_CONTACT_FLAG_FRICTION_ANCHOR = 16,
};
/// ManifoldContactPoint collects and maintains persistent contactpoints.

1
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp
View File

@@ -279,6 +279,7 @@ void btPersistentManifold::refreshContactPoints(const btTransform& trA,const btT
removeContactPoint(i);
} else
{
//todo: friction anchor may require the contact to be around a bit longer
//contact also becomes invalid when relative movement orthogonal to normal exceeds margin
projectedPoint = manifoldPoint.m_positionWorldOnA - manifoldPoint.m_normalWorldOnB * manifoldPoint.m_distance1;
projectedDifference = manifoldPoint.m_positionWorldOnB - projectedPoint;

55
src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h
View File

@@ -172,39 +172,54 @@ public:
btAssert(m_pointCache[lastUsedIndex].m_userPersistentData==0);
m_cachedPoints--;
}
void replaceContactPoint(const btManifoldPoint& newPoint,int insertIndex)
void replaceContactPoint(const btManifoldPoint& newPoint, int insertIndex)
{
btAssert(validContactDistance(newPoint));
#define MAINTAIN_PERSISTENCY 1
#ifdef MAINTAIN_PERSISTENCY
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
// bool isLateralFrictionInitialized = m_pointCache[insertIndex].m_lateralFrictionInitialized;
btAssert(lifeTime>=0);
void* cache = m_pointCache[insertIndex].m_userPersistentData;
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
int lifeTime = m_pointCache[insertIndex].getLifeTime();
btScalar appliedImpulse = m_pointCache[insertIndex].m_appliedImpulse;
btScalar appliedLateralImpulse1 = m_pointCache[insertIndex].m_appliedImpulseLateral1;
btScalar appliedLateralImpulse2 = m_pointCache[insertIndex].m_appliedImpulseLateral2;
bool replacePoint = true;
///we keep existing contact points for friction anchors
///if the friction force is within the Coulomb friction cone
if (newPoint.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
// printf("appliedImpulse=%f\n", appliedImpulse);
// printf("appliedLateralImpulse1=%f\n", appliedLateralImpulse1);
// printf("appliedLateralImpulse2=%f\n", appliedLateralImpulse2);
// printf("mu = %f\n", m_pointCache[insertIndex].m_combinedFriction);
btScalar mu = m_pointCache[insertIndex].m_combinedFriction;
btScalar eps = 0; //we could allow to enlarge or shrink the tolerance to check against the friction cone a bit, say 1e-7
btScalar a = appliedLateralImpulse1 * appliedLateralImpulse1 + appliedLateralImpulse2 * appliedLateralImpulse2;
btScalar b = eps + mu * appliedImpulse;
b = b * b;
replacePoint = (a) > (b);
}
if (replacePoint)
{
btAssert(lifeTime >= 0);
void* cache = m_pointCache[insertIndex].m_userPersistentData;
m_pointCache[insertIndex] = newPoint;
m_pointCache[insertIndex].m_userPersistentData = cache;
m_pointCache[insertIndex].m_appliedImpulse = appliedImpulse;
m_pointCache[insertIndex].m_appliedImpulseLateral1 = appliedLateralImpulse1;
m_pointCache[insertIndex].m_appliedImpulseLateral2 = appliedLateralImpulse2;
}
m_pointCache[insertIndex].m_lifeTime = lifeTime;
#else
clearUserCache(m_pointCache[insertIndex]);
m_pointCache[insertIndex] = newPoint;
#endif
}
bool validContactDistance(const btManifoldPoint& pt) const
{
return pt.m_distance1 <= getContactBreakingThreshold();

13
src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h
View File

@@ -43,10 +43,13 @@ struct btContactSolverInfoData
btScalar m_restitution;
int m_numIterations;
btScalar m_maxErrorReduction;
btScalar m_sor;
btScalar m_erp;//used as Baumgarte factor
btScalar m_erp2;//used in Split Impulse
btScalar m_globalCfm;//constraint force mixing
btScalar m_sor;//successive over-relaxation term
btScalar m_erp;//error reduction for non-contact constraints
btScalar m_erp2;//error reduction for contact constraints
btScalar m_globalCfm;//constraint force mixing for contacts and non-contacts
btScalar m_frictionERP;//error reduction for friction constraints
btScalar m_frictionCFM;//constraint force mixing for friction constraints
int m_splitImpulse;
btScalar m_splitImpulsePenetrationThreshold;
btScalar m_splitImpulseTurnErp;
@@ -79,6 +82,8 @@ struct btContactSolverInfo : public btContactSolverInfoData
m_erp = btScalar(0.2);
m_erp2 = btScalar(0.2);
m_globalCfm = btScalar(0.);
m_frictionERP = btScalar(0.2);//positional friction 'anchors' are disabled by default
m_frictionCFM = btScalar(0.);
m_sor = btScalar(1.);
m_splitImpulse = true;
m_splitImpulsePenetrationThreshold = -.04f;

34
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

@@ -534,7 +534,7 @@ void btSequentialImpulseConstraintSolver::applyAnisotropicFriction(btCollisionOb
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity, btScalar cfmSlip)
void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
@@ -612,7 +612,17 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
btScalar velocityError = desiredVelocity - rel_vel;
btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
solverConstraint.m_rhs = velocityImpulse;
btScalar penetrationImpulse = btScalar(0);
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
btScalar distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(normalAxis);
btScalar positionalError = -distance * infoGlobal.m_frictionERP/infoGlobal.m_timeStep;
penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
}
solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_cfm = cfmSlip;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
@@ -621,12 +631,12 @@ void btSequentialImpulseConstraintSolver::setupFrictionConstraint(btSolverConstr
}
}
btSolverConstraint& btSequentialImpulseConstraintSolver::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, btScalar cfmSlip)
btSolverConstraint& btSequentialImpulseConstraintSolver::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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip)
{
btSolverConstraint& solverConstraint = m_tmpSolverContactFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex;
setupFrictionConstraint(solverConstraint, normalAxis, solverBodyIdA, solverBodyIdB, cp, rel_pos1, rel_pos2,
colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
colObj0, colObj1, relaxation, infoGlobal, desiredVelocity, cfmSlip);
return solverConstraint;
}
@@ -1168,6 +1178,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
///In that case, you can set the target relative motion in each friction direction (cp.m_contactMotion1 and cp.m_contactMotion2)
///this will give a conveyor belt effect
///
if (!(infoGlobal.m_solverMode & SOLVER_ENABLE_FRICTION_DIRECTION_CACHING) || !(cp.m_contactPointFlags&BT_CONTACT_FLAG_LATERAL_FRICTION_INITIALIZED))
{
cp.m_lateralFrictionDir1 = vel - cp.m_normalWorldOnB * rel_vel;
@@ -1177,7 +1188,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir1 *= 1.f/btSqrt(lat_rel_vel);
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,infoGlobal);
if((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
@@ -1185,7 +1196,7 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
cp.m_lateralFrictionDir2.normalize();//??
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
}
} else
@@ -1194,13 +1205,13 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
{
applyAnisotropicFriction(colObj0,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir2,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal);
}
@@ -1212,10 +1223,10 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
} else
{
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation,cp.m_contactMotion1, cp.m_frictionCFM);
addFrictionConstraint(cp.m_lateralFrictionDir1,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion1, cp.m_frictionCFM);
if ((infoGlobal.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, cp.m_contactMotion2, cp.m_frictionCFM);
addFrictionConstraint(cp.m_lateralFrictionDir2,solverBodyIdA,solverBodyIdB,frictionIndex,cp,rel_pos1,rel_pos2,colObj0,colObj1, relaxation, infoGlobal, cp.m_contactMotion2, cp.m_frictionCFM);
}
setFrictionConstraintImpulse( solverConstraint, solverBodyIdA, solverBodyIdB, cp, infoGlobal);
@@ -1526,7 +1537,8 @@ btScalar btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySetup(btCol
sum += iMJaB.dot(solverConstraint.m_relpos2CrossNormal);
btScalar fsum = btFabs(sum);
btAssert(fsum > SIMD_EPSILON);
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?btScalar(1.)/sum : 0.f;
btScalar sorRelaxation = 1.f;//todo: get from globalInfo?
solverConstraint.m_jacDiagABInv = fsum>SIMD_EPSILON?sorRelaxation/sum : 0.f;
}

3
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -62,6 +62,7 @@ protected:
void setupFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
@@ -69,7 +70,7 @@ protected:
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& 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, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity=0., btScalar cfmSlip=0.);
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);

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

@@ -50,7 +50,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
//solve featherstone normal contact
for (int j0=0;j0<m_multiBodyNormalContactConstraints.size();j0++)
{
int index = iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
int index = j0;//iteration&1? j0 : m_multiBodyNormalContactConstraints.size()-1-j0;
btMultiBodySolverConstraint& constraint = m_multiBodyNormalContactConstraints[index];
btScalar residual = 0.f;
@@ -74,7 +74,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl
{
if (iteration < infoGlobal.m_numIterations)
{
int index = iteration&1? j1 : m_multiBodyFrictionContactConstraints.size()-1-j1;
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;
@@ -244,32 +244,39 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
//cfm = 1 / ( dt * kp + kd )
//erp = dt * kp / ( dt * kp + kd )
btScalar cfm = infoGlobal.m_globalCfm;
btScalar erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
cfm *= invTimeStep;
btScalar cfm;
btScalar erp;
if (isFriction)
{
cfm = infoGlobal.m_frictionCFM;
erp = infoGlobal.m_frictionERP;
} else
{
cfm = infoGlobal.m_globalCfm;
erp = infoGlobal.m_erp2;
if ((cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM) || (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP))
{
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_CFM)
cfm = cp.m_contactCFM;
if (cp.m_contactPointFlags&BT_CONTACT_FLAG_HAS_CONTACT_ERP)
erp = cp.m_contactERP;
} else
{
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_CONTACT_STIFFNESS_DAMPING)
{
btScalar denom = ( infoGlobal.m_timeStep * cp.m_combinedContactStiffness1 + cp.m_combinedContactDamping1 );
if (denom < SIMD_EPSILON)
{
denom = SIMD_EPSILON;
}
cfm = btScalar(1) / denom;
erp = (infoGlobal.m_timeStep * cp.m_combinedContactStiffness1) / denom;
}
}
}
cfm *= invTimeStep;
if (multiBodyA)
{
@@ -429,8 +436,16 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btScalar restitution = 0.f;
btScalar penetration = isFriction? 0 : cp.getDistance()+infoGlobal.m_linearSlop;
btScalar distance = 0;
if (!isFriction)
{
distance = cp.getDistance()+infoGlobal.m_linearSlop;
} else
{
distance = (cp.getPositionWorldOnA() - cp.getPositionWorldOnB()).dot(contactNormal);
}
btScalar rel_vel = 0.f;
int ndofA = 0;
int ndofB = 0;
@@ -526,14 +541,17 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
btScalar positionalError = 0.f;
btScalar velocityError = restitution - rel_vel;// * damping; //note for friction restitution is always set to 0 (check above) so it is acutally velocityError = -rel_vel for friction
if (penetration>0)
if (distance>0 && !isFriction)
{
positionalError = 0;
velocityError -= penetration / infoGlobal.m_timeStep;
velocityError -= distance / infoGlobal.m_timeStep;
} else
{
positionalError = -penetration * erp/infoGlobal.m_timeStep;
if (cp.m_contactPointFlags & BT_CONTACT_FLAG_FRICTION_ANCHOR)
{
positionalError = -distance * erp/infoGlobal.m_timeStep;
}
}
btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
@@ -560,7 +578,7 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
}
else
{
solverConstraint.m_rhs = velocityImpulse;
solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
solverConstraint.m_rhsPenetration = 0.f;
solverConstraint.m_lowerLimit = -solverConstraint.m_friction;
solverConstraint.m_upperLimit = solverConstraint.m_friction;
@@ -1028,12 +1046,18 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
applyAnisotropicFriction(colObj1,cp.m_lateralFrictionDir1,btCollisionObject::CF_ANISOTROPIC_FRICTION);
addMultiBodyFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal);
if (rollingFriction > 0)
if (rollingFriction > 0 )
{
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);
if (cp.m_combinedSpinningFriction>0)
{
addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
}
if (cp.m_combinedRollingFriction>0)
{
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--;
}

111
test/RobotClientAPI/SlopeFrictionMain.cpp Normal file
View File

@@ -0,0 +1,111 @@
//todo: turn this into a gtest, comparing maximal and reduced coordinates contact behavior etc
#include "b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
#include <string.h>
#include <stdio.h>
#include <assert.h>
#define ASSERT_EQ(a,b) assert((a)==(b));
#include "MinitaurSetup.h"
#define NUM_SIM 1
int main(int argc, char* argv[])
{
b3RobotSimulatorClientAPI* sims[2];
b3Scalar fixedTimeStep = 1./240.;
for (int i=0;i<NUM_SIM;i++)
{
b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
sims[i] = sim;
sim->connect(eCONNECT_GUI);//eCONNECT_GUI);//DIRECT);
//Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example:
//sim->connect(eCONNECT_UDP, "localhost", 1234);
sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
// sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
//sim->setTimeOut(3);
//syncBodies is only needed when connecting to an existing physics server that has already some bodies
sim->syncBodies();
sim->setTimeStep(fixedTimeStep);
b3Quaternion q = sim->getQuaternionFromEuler(b3MakeVector3(0.1,0.2,0.3));
b3Vector3 rpy;
rpy = sim->getEulerFromQuaternion(q);
sim->setGravity(b3MakeVector3(0,0,-9.8));
//int blockId = sim->loadURDF("cube.urdf");
//b3BodyInfo bodyInfo;
//sim->getBodyInfo(blockId,&bodyInfo);
b3Quaternion orn;
orn.setEulerZYX(0,B3_PI*0.23,0);
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0,0,0);
args.m_startOrientation = orn;
args.m_useMultiBody = i==0? false : true;//true : false;//false : true;
sim->loadURDF("plane.urdf",args);
args.m_startPosition.setValue(0,0,0.66);
args.m_startOrientation.setEulerZYX(0,B3_PI*0.23,0);
sim->loadURDF("cube_soft.urdf",args);
double distance = 1.5;
double yaw = 50;
sim->resetDebugVisualizerCamera(distance,yaw,20,b3MakeVector3(0,0,0.1));
sim->setRealTimeSimulation(false);
}
int enableSim = 1;
while (sims[0]->canSubmitCommand())
{
#if 0
b3KeyboardEventsData keyEvents;
sim->getKeyboardEvents(&keyEvents);
if (keyEvents.m_numKeyboardEvents)
{
//printf("num key events = %d]\n", keyEvents.m_numKeyboardEvents);
//m_keyState is a flag combination of eButtonIsDown,eButtonTriggered, eButtonReleased
for (int i=0;i<keyEvents.m_numKeyboardEvents;i++)
{
if (keyEvents.m_keyboardEvents[i].m_keyCode=='i' && keyEvents.m_keyboardEvents[i].m_keyState & eButtonTriggered)
{
enableSim = !enableSim;
}
}
}
#endif
for (int i=0;i<NUM_SIM;i++)
{
sims[i]->setGravity(b3MakeVector3(0,0,-10));
//printf(".");
if (enableSim)
{
sims[i]->stepSimulation();
}
}
b3Clock::usleep(1000.*1000.*fixedTimeStep);
}
printf("sim->disconnect\n");
for (int i=0;i<NUM_SIM;i++)
{
sims[i]->disconnect();
printf("delete sim\n");
delete sims[i];
}
printf("exit\n");
}