diff --git a/build3/premake4_arm64 b/build3/premake4_arm64 new file mode 100755 index 000000000..3b7ee4ab5 Binary files /dev/null and b/build3/premake4_arm64 differ diff --git a/data/cube_soft.urdf b/data/cube_soft.urdf index af40dd379..6354ab3b6 100644 --- a/data/cube_soft.urdf +++ b/data/cube_soft.urdf @@ -2,7 +2,8 @@ - + + diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 30f35963a..d4065b8fe 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -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); + } } diff --git a/examples/Importers/ImportURDFDemo/URDFJointTypes.h b/examples/Importers/ImportURDFDemo/URDFJointTypes.h index cc39e8913..3a11d981e 100644 --- a/examples/Importers/ImportURDFDemo/URDFJointTypes.h +++ b/examples/Importers/ImportURDFDemo/URDFJointTypes.h @@ -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 diff --git a/examples/Importers/ImportURDFDemo/UrdfParser.cpp b/examples/Importers/ImportURDFDemo/UrdfParser.cpp index ad5247710..c273a8d9c 100644 --- a/examples/Importers/ImportURDFDemo/UrdfParser.cpp +++ b/examples/Importers/ImportURDFDemo/UrdfParser.cpp @@ -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; + } } { diff --git a/examples/RobotSimulator/RobotSimulatorMain.cpp b/examples/RobotSimulator/RobotSimulatorMain.cpp index 7ab3f3e70..558c5e9de 100644 --- a/examples/RobotSimulator/RobotSimulatorMain.cpp +++ b/examples/RobotSimulator/RobotSimulatorMain.cpp @@ -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;istartStateLogging(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); } diff --git a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp index 8b2834a2f..64b062964 100644 --- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp +++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp @@ -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) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index a7d23f68c..5ac6ea74a 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -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); } diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index c5a48efaa..b729392d3 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -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; @@ -1824,7 +1826,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (clientCmd.m_stateLoggingArguments.m_logType == STATE_LOGGING_VIDEO_MP4) { - if (clientCmd.m_stateLoggingArguments.m_fileName) + //if (clientCmd.m_stateLoggingArguments.m_fileName) { int loggerUid = m_data->m_stateLoggersUniqueId++; VideoMP4Loggger* logger = new VideoMP4Loggger(loggerUid,clientCmd.m_stateLoggingArguments.m_fileName,this->m_data->m_guiHelper); @@ -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) diff --git a/src/BulletCollision/CollisionDispatch/btCollisionObject.h b/src/BulletCollision/CollisionDispatch/btCollisionObject.h index 0cae21000..598ad2229 100644 --- a/src/BulletCollision/CollisionDispatch/btCollisionObject.h +++ b/src/BulletCollision/CollisionDispatch/btCollisionObject.h @@ -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 diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index be8e51d52..621292e22 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -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); diff --git a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h index 04ab54ed9..571ad2c5f 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h +++ b/src/BulletCollision/NarrowPhaseCollision/btManifoldPoint.h @@ -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. diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp index 4d92e853d..c5bd7c16f 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.cpp @@ -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; diff --git a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h index 3b82be192..a351cb3c2 100644 --- a/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h +++ b/src/BulletCollision/NarrowPhaseCollision/btPersistentManifold.h @@ -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(); diff --git a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h index 739b066fe..f3d4d45af 100644 --- a/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h +++ b/src/BulletDynamics/ConstraintSolver/btContactSolverInfo.h @@ -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; diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp index 982650da2..7a0ed924d 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp @@ -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; } diff --git a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h index 0dd31d142..b81537138 100644 --- a/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h +++ b/src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h @@ -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); diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index e28809fd4..e6bbfb5d7 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -50,7 +50,7 @@ btScalar btMultiBodyConstraintSolver::solveSingleIteration(int iteration, btColl //solve featherstone normal contact for (int j0=0;j00) + 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--; } diff --git a/test/RobotClientAPI/SlopeFrictionMain.cpp b/test/RobotClientAPI/SlopeFrictionMain.cpp new file mode 100644 index 000000000..f0d802667 --- /dev/null +++ b/test/RobotClientAPI/SlopeFrictionMain.cpp @@ -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 +#include +#include +#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;iconnect(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;isetGravity(b3MakeVector3(0,0,-10)); + //printf("."); + if (enableSim) + { + sims[i]->stepSimulation(); + } + } + b3Clock::usleep(1000.*1000.*fixedTimeStep); + } + + printf("sim->disconnect\n"); + + for (int i=0;idisconnect(); + printf("delete sim\n"); + delete sims[i]; + + } + + + printf("exit\n"); + +} + +