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

separate spinning and rolling friction coefficients, exposed in URDF as spinning_friction / m_rolling_friction

Browse Source 
improvements in VR demo, add grasper etc.
This commit is contained in:
Erwin Coumans
2016-09-16 00:57:00 +01:00
parent 1d88cf71e4
commit 567b003654
14 changed files with 92 additions and 74 deletions
Download Patch File Download Diff File Expand all files Collapse all files

4
data/cube_gripper_left.urdf
View File

@@ -6,10 +6,8 @@
<link name="baseLink"> <link name="baseLink">
<contact> <contact>
<lateral_friction value="1.0"/> <lateral_friction value="1.0"/>
<rolling_friction value="0.3"/> <spinning_friction value="0.3"/>
<inertia_scaling value="3.0"/> <inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>

4
data/cube_gripper_right.urdf
View File

@@ -6,10 +6,8 @@
<link name="baseLink"> <link name="baseLink">
<contact> <contact>
<lateral_friction value="1.0"/> <lateral_friction value="1.0"/>
<rolling_friction value="0.3"/> <spinning_friction value="0.3"/>
<inertia_scaling value="3.0"/> <inertia_scaling value="3.0"/>
<contact_cfm value="0.0"/>
<contact_erp value="1.0"/>
</contact> </contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>

2
data/cube_small.urdf
View File

@@ -3,7 +3,7 @@
<link name="baseLink"> <link name="baseLink">
<contact> <contact>
<lateral_friction value="1.0"/> <lateral_friction value="1.0"/>
<rolling_friction value="0.1"/> <spinning_friction value="0.1"/>
<inertia_scaling value="3.0"/> <inertia_scaling value="3.0"/>
</contact> </contact>
<inertial> <inertial>

3
data/plane.urdf
View File

@@ -1,9 +1,6 @@
<?xml version="0.0" ?> <?xml version="0.0" ?>
<robot name="cube.urdf"> <robot name="cube.urdf">
<link name="baseLink"> <link name="baseLink">
<contact>
<rolling_friction value="0.3"/>
</contact>
<inertial> <inertial>
<origin rpy="0 0 0" xyz="0 0 0"/> <origin rpy="0 0 0" xyz="0 0 0"/>
<mass value=".0"/> <mass value=".0"/>

1
data/pr2_gripper.urdf
View File

@@ -33,6 +33,7 @@
<link name="left_gripper"> <link name="left_gripper">
<contact> <contact>
<lateral_friction value="50.0"/> <lateral_friction value="50.0"/>
<spinning_friction value="0.1"/>
</contact> </contact>
<visual> <visual>
<origin rpy="0.0 0 0" xyz="0 0 0"/> <origin rpy="0.0 0 0" xyz="0 0 0"/>

2
examples/CommonInterfaces/CommonExampleInterface.h
View File

@@ -46,7 +46,7 @@ public:
virtual bool mouseButtonCallback(int button, int state, float x, float y)=0; virtual bool mouseButtonCallback(int button, int state, float x, float y)=0;
virtual bool keyboardCallback(int key, int state)=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]){} virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]){}
}; };

49
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -27,6 +27,8 @@
#include "../CommonInterfaces/CommonGUIHelperInterface.h" #include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "SharedMemoryCommands.h" #include "SharedMemoryCommands.h"
btVector3 gLastPickPos(0, 0, 0);
struct UrdfLinkNameMapUtil struct UrdfLinkNameMapUtil
{ {
btMultiBody* m_mb; btMultiBody* m_mb;
@@ -383,7 +385,7 @@ struct PhysicsServerCommandProcessorInternalData
btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; btMultiBodyFixedConstraint* m_gripperRigidbodyFixed;
btMultiBody* m_gripperMultiBody; btMultiBody* m_gripperMultiBody;
int m_huskyId;
CommandLogger* m_commandLogger; CommandLogger* m_commandLogger;
CommandLogPlayback* m_logPlayback; CommandLogPlayback* m_logPlayback;
@@ -435,6 +437,7 @@ struct PhysicsServerCommandProcessorInternalData
m_gripperRigidbodyFixed(0), m_gripperRigidbodyFixed(0),
m_gripperMultiBody(0), m_gripperMultiBody(0),
m_allowRealTimeSimulation(false), m_allowRealTimeSimulation(false),
m_huskyId(0),
m_commandLogger(0), m_commandLogger(0),
m_logPlayback(0), m_logPlayback(0),
m_physicsDeltaTime(1./240.), m_physicsDeltaTime(1./240.),
@@ -1874,10 +1877,15 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
{ {
applyJointDamping(i); 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); if (m_data->m_numSimulationSubSteps > 0)
else {
m_data->m_dynamicsWorld->stepSimulation(m_data->m_physicsDeltaTime,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; SharedMemoryStatus& serverCmd =serverStatusOut;
serverCmd.m_type = CMD_STEP_FORWARD_SIMULATION_COMPLETED; 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) bool PhysicsServerCommandProcessor::pickBody(const btVector3& rayFromWorld, const btVector3& rayToWorld)
{ {
@@ -2832,6 +2840,7 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName)
btVector3 gVRGripperPos(0,0,0); btVector3 gVRGripperPos(0,0,0);
btQuaternion gVRGripperOrn(0,0,0,1); btQuaternion gVRGripperOrn(0,0,0,1);
btScalar gVRGripperAnalog = 0;
bool gVRGripperClosed = false; bool gVRGripperClosed = false;
@@ -2860,7 +2869,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
{ {
int bodyId = 0; 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); InteralBodyData* parentBody = m_data->getHandle(bodyId);
if (parentBody->m_multiBody) if (parentBody->m_multiBody)
@@ -2878,14 +2887,25 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec)
m_data->m_gripperMultiBody = parentBody->m_multiBody; m_data->m_gripperMultiBody = parentBody->m_multiBody;
if (m_data->m_gripperMultiBody->getNumLinks() > 2) if (m_data->m_gripperMultiBody->getNumLinks() > 2)
{ {
m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI); m_data->m_gripperMultiBody->setJointPos(0, 0);
m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI); m_data->m_gripperMultiBody->setJointPos(2, 0);
} }
m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.); m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.);
btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld;
world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); 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); motor->setErp(0.01);
if (gVRGripperClosed) motor->setPositionTarget(0.1+(1-gVRGripperAnalog)*SIMD_HALF_PI*0.5, 1);
{
motor->setPositionTarget(0.2, 1);
}
else
{
motor->setPositionTarget(SIMD_HALF_PI, 1);
}
motor->setVelocityTarget(0, 0.1); motor->setVelocityTarget(0, 0.1);
btScalar maxImp = 1550.*m_data->m_physicsDeltaTime; btScalar maxImp = 1550.*m_data->m_physicsDeltaTime;
motor->setMaxAppliedImpulse(maxImp); motor->setMaxAppliedImpulse(maxImp);

17
examples/SharedMemory/PhysicsServerExample.cpp
View File

@@ -18,6 +18,8 @@
extern btVector3 gLastPickPos; extern btVector3 gLastPickPos;
btVector3 gVRTeleportPos(0,0,0); btVector3 gVRTeleportPos(0,0,0);
btQuaternion gVRTeleportOrn(0, 0, 0,1);
extern bool gVRGripperClosed; extern bool gVRGripperClosed;
bool gDebugRenderToggle = false; bool gDebugRenderToggle = false;
@@ -556,7 +558,7 @@ public:
btVector3 getRayTo(int x,int y); btVector3 getRayTo(int x,int y);
virtual void vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orientation[4]); 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) virtual bool mouseMoveCallback(float x,float y)
{ {
@@ -894,6 +896,7 @@ void PhysicsServerExample::renderScene()
///debug rendering ///debug rendering
//m_args[0].m_cs->lock(); //m_args[0].m_cs->lock();
//gVRTeleportPos[0] += 0.01;
vrOffset[12]=-gVRTeleportPos[0]; vrOffset[12]=-gVRTeleportPos[0];
vrOffset[13]=-gVRTeleportPos[1]; vrOffset[13]=-gVRTeleportPos[1];
vrOffset[14]=-gVRTeleportPos[2]; 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]) 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) if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS)
return; return;
if (gGraspingController < 0)
gGraspingController = controllerId;
if (controllerId != gGraspingController) if (controllerId != gGraspingController)
{ {
@@ -1151,10 +1156,15 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt
extern btVector3 gVRGripperPos; extern btVector3 gVRGripperPos;
extern btQuaternion gVRGripperOrn; 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) if (controllerId <= 0 || controllerId >= MAX_VR_CONTROLLERS)
{ {
printf("Controller Id exceeds max: %d > %d", 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) if (controllerId == gGraspingController)
{ {
gVRGripperAnalog = analogAxis;
gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]);
btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]); 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); gVRGripperOrn = orgOrn*btQuaternion(btVector3(0, 0, 1), SIMD_HALF_PI)*btQuaternion(btVector3(0, 1, 0), SIMD_HALF_PI);

7
examples/StandaloneMain/hellovr_opengl_main.cpp
View File

@@ -713,14 +713,17 @@ bool CMainApplication::HandleInput()
} }
else else
{ {
// printf("Device MOVED: %d\n", unDevice); // printf("Device MOVED: %d\n", unDevice);
sExample->vrControllerMoveCallback(unDevice, pos, orn); sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
} }
} }
else else
{ {
if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller ) if( m_pHMD->GetTrackedDeviceClass( unDevice) == vr::TrackedDeviceClass_Controller )
{ {
b3Transform tr; b3Transform tr;
getControllerTransform(unDevice, tr); getControllerTransform(unDevice, tr);
float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] }; float pos[3] = { tr.getOrigin()[0], tr.getOrigin()[1], tr.getOrigin()[2] };
@@ -741,7 +744,7 @@ bool CMainApplication::HandleInput()
} else } else
{ {
sExample->vrControllerMoveCallback(unDevice, pos, orn); sExample->vrControllerMoveCallback(unDevice, pos, orn, state.rAxis[1].x);
} }
} }
} }

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

@@ -135,6 +135,7 @@ void btManifoldResult::addContactPoint(const btVector3& normalOnBInWorld,const b
newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject()); newPt.m_combinedFriction = calculateCombinedFriction(m_body0Wrap->getCollisionObject(),m_body1Wrap->getCollisionObject());
newPt.m_combinedRestitution = calculateCombinedRestitution(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_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) || if ( (m_body0Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING) ||
(m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING)) (m_body1Wrap->getCollisionObject()->getCollisionFlags()& btCollisionObject::CF_HAS_CONTACT_STIFFNESS_DAMPING))

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

@@ -627,8 +627,8 @@ btSolverConstraint& btSequentialImpulseConstraintSolver::addFrictionConstraint(c
} }
void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB, void btSequentialImpulseConstraintSolver::setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis1,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip) btScalar desiredVelocity, btScalar cfmSlip)
@@ -647,8 +647,8 @@ void btSequentialImpulseConstraintSolver::setupRollingFrictionConstraint( btSolv
solverConstraint.m_solverBodyIdA = solverBodyIdA; solverConstraint.m_solverBodyIdA = solverBodyIdA;
solverConstraint.m_solverBodyIdB = solverBodyIdB; solverConstraint.m_solverBodyIdB = solverBodyIdB;
solverConstraint.m_friction = cp.m_combinedRollingFriction; solverConstraint.m_friction = combinedTorsionalFriction;
solverConstraint.m_originalContactPoint = 0; solverConstraint.m_originalContactPoint = 0;
solverConstraint.m_appliedImpulse = 0.f; solverConstraint.m_appliedImpulse = 0.f;
solverConstraint.m_appliedPushImpulse = 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(); btSolverConstraint& solverConstraint = m_tmpSolverContactRollingFrictionConstraintPool.expandNonInitializing();
solverConstraint.m_frictionIndex = frictionIndex; 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); colObj0, colObj1, relaxation, desiredVelocity, cfmSlip);
return solverConstraint; return solverConstraint;
} }
@@ -1069,28 +1069,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
solverConstraint.m_frictionIndex = m_tmpSolverContactFrictionConstraintPool.size(); 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)) if ((cp.m_combinedRollingFriction>0.f) && (rollingFriction>0))
{ {
//only a single rollingFriction per manifold
//rollingFriction--;
if (relAngVel.length()>infoGlobal.m_singleAxisRollingFrictionThreshold)
{ {
relAngVel.normalize(); addTorsionalFrictionConstraint(cp.m_normalWorldOnB,solverBodyIdA,solverBodyIdB,frictionIndex,cp,cp.m_combinedSpinningFriction, rel_pos1,rel_pos2,colObj0,colObj1, relaxation);
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);
btVector3 axis0,axis1; btVector3 axis0,axis1;
btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1); btPlaneSpace1(cp.m_normalWorldOnB,axis0,axis1);
axis0.normalize(); axis0.normalize();
@@ -1101,9 +1084,11 @@ void btSequentialImpulseConstraintSolver::convertContact(btPersistentManifold* m
applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj0,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION); applyAnisotropicFriction(colObj1,axis1,btCollisionObject::CF_ANISOTROPIC_ROLLING_FRICTION);
if (axis0.length()>0.001) 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) 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);
} }
} }

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

@@ -54,13 +54,13 @@ protected:
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.); btScalar desiredVelocity=0., btScalar cfmSlip=0.);
void setupRollingFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB, void setupTorsionalFrictionConstraint( btSolverConstraint& solverConstraint, const btVector3& normalAxis,int solverBodyIdA,int solverBodyIdB,
btManifoldPoint& cp,const btVector3& rel_pos1,const btVector3& rel_pos2, btManifoldPoint& cp,btScalar combinedTorsionalFriction, const btVector3& rel_pos1,const btVector3& rel_pos2,
btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation, btCollisionObject* colObj0,btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity=0., btScalar cfmSlip=0.); 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, 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, void setupContactConstraint(btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,

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

@@ -557,9 +557,11 @@ void btMultiBodyConstraintSolver::setupMultiBodyContactConstraint(btMultiBodySol
} }
void btMultiBodyConstraintSolver::setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, void btMultiBodyConstraintSolver::setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& constraintNormal, const btVector3& constraintNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation, btScalar& relaxation,
bool isFriction, btScalar desiredVelocity, btScalar cfmSlip) 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) if(!isFriction)
{ {
@@ -860,7 +862,9 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyFrictionCo
return solverConstraint; 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"); BT_PROFILE("addMultiBodyRollingFrictionConstraint");
btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing(); btMultiBodySolverConstraint& solverConstraint = m_multiBodyFrictionContactConstraints.expandNonInitializing();
@@ -891,7 +895,7 @@ btMultiBodySolverConstraint& btMultiBodyConstraintSolver::addMultiBodyRollingFri
solverConstraint.m_originalContactPoint = &cp; 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; return solverConstraint;
} }
@@ -1010,9 +1014,9 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold*
if (rollingFriction > 0) if (rollingFriction > 0)
{ {
addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); addMultiBodyTorsionalFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,cp.m_combinedSpinningFriction, colObj0,colObj1, relaxation,infoGlobal);
//addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
//addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); addMultiBodyTorsionalFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,cp.m_combinedRollingFriction, colObj0,colObj1, relaxation,infoGlobal);
rollingFriction--; rollingFriction--;
} }

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

@@ -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& 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, void setupMultiBodyJointLimitConstraint(btMultiBodySolverConstraint& constraintRow,
btScalar* jacA,btScalar* jacB, btScalar* jacA,btScalar* jacB,
@@ -63,9 +65,12 @@ protected:
btScalar& relaxation, btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);
void setupMultiBodyRollingFrictionConstraint(btMultiBodySolverConstraint& solverConstraint, //either rolling or spinning friction
void setupMultiBodyTorsionalFrictionConstraint(btMultiBodySolverConstraint& solverConstraint,
const btVector3& contactNormal, const btVector3& contactNormal,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btManifoldPoint& cp,
btScalar combinedTorsionalFriction,
const btContactSolverInfo& infoGlobal,
btScalar& relaxation, btScalar& relaxation,
bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0); bool isFriction, btScalar desiredVelocity=0, btScalar cfmSlip=0);