diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 9cefeb7d2..0a9c03a51 100644 --- a/build_visual_studio.bat +++ b/build_visual_studio.bat @@ -2,9 +2,9 @@ rem premake4 --with-pe vs2010 rem premake4 --bullet2demos vs2010 cd build3 -rem premake4 --enable_openvr --targetdir="../bin" vs2010 +premake4 --enable_openvr --targetdir="../bin" vs2010 -premake4 --enable_pybullet --python_include_dir="c:/python34/include" --python_lib_dir="c:/python34/libs" --enable_openvr --targetdir="../bin" vs2010 +rem premake4 --enable_pybullet --python_include_dir="C:/Python-3.6.0a3/include" --python_lib_dir="C:/Python-3.6.0a3/libs" --enable_openvr --targetdir="../bin" vs2010 rem premake4 --enable_openvr --enable_pybullet --targetdir="../bin" vs2010 rem premake4 --targetdir="../server2bin" vs2010 rem cd vs2010 diff --git a/data/cube_small.urdf b/data/cube_small.urdf index a0e3cc609..d28667c8e 100644 --- a/data/cube_small.urdf +++ b/data/cube_small.urdf @@ -3,9 +3,8 @@ - + - diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf index 33d833629..b9fed9e53 100644 --- a/data/pr2_gripper.urdf +++ b/data/pr2_gripper.urdf @@ -31,6 +31,9 @@ + + + @@ -55,6 +58,9 @@ + + + @@ -63,9 +69,9 @@ - + - + @@ -82,6 +88,9 @@ + + + @@ -106,6 +115,9 @@ + + + @@ -114,9 +126,9 @@ - + - + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f45610c15..22505bd69 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -525,7 +525,7 @@ PhysicsServerCommandProcessor::PhysicsServerCommandProcessor() m_data = new PhysicsServerCommandProcessorInternalData(); createEmptyDynamicsWorld(); - m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0001; + m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.0; } @@ -2823,7 +2823,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI); m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI); } - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.); + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(1.); btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); } @@ -2844,7 +2844,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr; if (motor) { - motor->setErp(0.005); + motor->setErp(0.01); if (gVRGripperClosed) { @@ -2855,7 +2855,7 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) motor->setPositionTarget(SIMD_HALF_PI, 1); } motor->setVelocityTarget(0, 0.1); - btScalar maxImp = 550.*m_data->m_physicsDeltaTime; + btScalar maxImp = 1550.*m_data->m_physicsDeltaTime; motor->setMaxAppliedImpulse(maxImp); } } diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index d75b8e4ff..9622c5aa4 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -1101,6 +1101,7 @@ class CommonExampleInterface* PhysicsServerCreateFunc(struct CommonExampleOpt } +static int gGraspingController = 2; void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int button, int state, float pos[4], float orn[4]) { @@ -1109,9 +1110,13 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt if (controllerId<0 || controllerId>=MAX_VR_CONTROLLERS) return; - if (button==1 && state==0) + + if (controllerId != gGraspingController) { - gVRTeleportPos = gLastPickPos; + if (button == 1 && state == 0) + { + gVRTeleportPos = gLastPickPos; + } } if (button==32 && state==0) { @@ -1124,7 +1129,7 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt m_args[0].m_isVrControllerTeleporting[controllerId] = true; } - if (controllerId == 3 && (button == 33)) + if (controllerId == gGraspingController && (button == 33)) { gVRGripperClosed =state; } @@ -1155,7 +1160,7 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); return; } - if (controllerId == 3) + if (controllerId == gGraspingController) { gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); btQuaternion orgOrn(orn[0], orn[1], orn[2], orn[3]); diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index d806ad27c..26f1a88f1 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -702,7 +702,7 @@ bool CMainApplication::HandleInput() glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); ///todo(erwincoumans) can't use reguar debug drawer, because physics/graphics are not in sync //add a special debug drawer that deals with this - //gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints + //gDebugDrawFlags = btIDebugDraw::DBG_DrawWireframe;// :DBG_DrawContactPoints //btIDebugDraw::DBG_DrawConstraintLimits+ //btIDebugDraw::DBG_DrawConstraints ; diff --git a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp index 0536a2ea2..48815d24f 100644 --- a/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp +++ b/src/BulletCollision/CollisionDispatch/btManifoldResult.cpp @@ -27,7 +27,7 @@ ContactAddedCallback gContactAddedCallback=0; ///User can override this material combiner by implementing gContactAddedCallback and setting body0->m_collisionFlags |= btCollisionObject::customMaterialCallback; btScalar btManifoldResult::calculateCombinedRollingFriction(const btCollisionObject* body0,const btCollisionObject* body1) { - btScalar friction = body0->getRollingFriction() * body1->getRollingFriction(); + btScalar friction = body0->getRollingFriction() * body1->getFriction() + body1->getRollingFriction() * body0->getFriction(); const btScalar MAX_FRICTION = btScalar(10.); if (friction < -MAX_FRICTION) diff --git a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp index 9633acaab..a70e82d70 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyConstraintSolver.cpp @@ -1011,8 +1011,8 @@ void btMultiBodyConstraintSolver::convertMultiBodyContact(btPersistentManifold* if (rollingFriction > 0) { addMultiBodyRollingFrictionConstraint(cp.m_normalWorldOnB,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); - addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + //addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir1,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); + //addMultiBodyRollingFrictionConstraint(cp.m_lateralFrictionDir2,manifold,frictionIndex,cp,colObj0,colObj1, relaxation,infoGlobal); rollingFriction--; }