diff --git a/build_visual_studio.bat b/build_visual_studio.bat index 08e6b70b6..9cefeb7d2 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 -premake4 --enable_openvr --targetdir="../bin" vs2010 +rem premake4 --enable_openvr --targetdir="../bin" vs2010 -rem premake4 --enable_pybullet --python_include_dir="c:/python-3.5.2/include" --python_lib_dir="c:/python-3.5.2/libs" --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_openvr --enable_pybullet --targetdir="../bin" vs2010 rem premake4 --targetdir="../server2bin" vs2010 rem cd vs2010 diff --git a/data/pr2_gripper.urdf b/data/pr2_gripper.urdf new file mode 100644 index 000000000..33d833629 --- /dev/null +++ b/data/pr2_gripper.urdf @@ -0,0 +1,126 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 94546637e..d2a83cc56 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -382,6 +382,7 @@ struct PhysicsServerCommandProcessorInternalData bool m_hasGround; btMultiBodyFixedConstraint* m_gripperRigidbodyFixed; + btMultiBody* m_gripperMultiBody; CommandLogger* m_commandLogger; CommandLogPlayback* m_logPlayback; @@ -432,6 +433,7 @@ struct PhysicsServerCommandProcessorInternalData PhysicsServerCommandProcessorInternalData() :m_hasGround(false), m_gripperRigidbodyFixed(0), + m_gripperMultiBody(0), m_allowRealTimeSimulation(true), m_commandLogger(0), m_logPlayback(0), @@ -2697,6 +2699,9 @@ void PhysicsServerCommandProcessor::replayFromLogFile(const char* fileName) btVector3 gVRGripperPos(0,0,0); btQuaternion gVRGripperOrn(0,0,0,1); +bool gVRGripperClosed = false; + + int gDroppedSimulationSteps = 0; int gNumSteps = 0; double gDtInSec = 0.f; @@ -2717,37 +2722,70 @@ void PhysicsServerCommandProcessor::stepSimulationRealTime(double dtInSec) loadUrdf("plane.urdf", btVector3(0, 0, 0), btQuaternion(0, 0, 0, 1), true, true, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - } - - if (0)//m_data->m_gripperRigidbodyFixed==0) - { - int bodyId = 0; - - loadUrdf("pr2_gripper.urdf",btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size()); - - InteralBodyData* parentBody = m_data->getHandle(bodyId); - if (parentBody->m_multiBody) + + if (m_data->m_gripperRigidbodyFixed == 0) { - parentBody->m_multiBody->setHasSelfCollision(1); - btVector3 pivotInParent(0,0,0); - btMatrix3x3 frameInParent; - frameInParent.setRotation(btQuaternion(0,0,0,1)); - - btVector3 pivotInChild(0,0,0); - btMatrix3x3 frameInChild; - frameInChild.setIdentity(); + int bodyId = 0; - m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody,-1,0,pivotInParent,pivotInChild,frameInParent,frameInChild); - m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.); - btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld; - world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); + if (loadUrdf("pr2_gripper.urdf", btVector3(0, 0, 1), btQuaternion(0, 0, 0, 1), true, false, &bodyId, &gBufferServerToClient[0], gBufferServerToClient.size())) + { + InteralBodyData* parentBody = m_data->getHandle(bodyId); + if (parentBody->m_multiBody) + { + parentBody->m_multiBody->setHasSelfCollision(1); + btVector3 pivotInParent(0, 0, 0); + btMatrix3x3 frameInParent; + frameInParent.setRotation(btQuaternion(0, 0, 0, 1)); + + btVector3 pivotInChild(0, 0, 0); + btMatrix3x3 frameInChild; + frameInChild.setIdentity(); + + m_data->m_gripperRigidbodyFixed = new btMultiBodyFixedConstraint(parentBody->m_multiBody, -1, 0, pivotInParent, pivotInChild, frameInParent, frameInChild); + m_data->m_gripperMultiBody = parentBody->m_multiBody; + if (m_data->m_gripperMultiBody->getNumLinks() > 2) + { + m_data->m_gripperMultiBody->setJointPos(0, SIMD_HALF_PI); + m_data->m_gripperMultiBody->setJointPos(2, SIMD_HALF_PI); + } + m_data->m_gripperRigidbodyFixed->setMaxAppliedImpulse(2.); + btMultiBodyDynamicsWorld* world = (btMultiBodyDynamicsWorld*)m_data->m_dynamicsWorld; + world->addMultiBodyConstraint(m_data->m_gripperRigidbodyFixed); + } + } } } - if (m_data->m_gripperRigidbodyFixed) + if (m_data->m_gripperRigidbodyFixed && m_data->m_gripperMultiBody) { m_data->m_gripperRigidbodyFixed->setFrameInB(btMatrix3x3(gVRGripperOrn)); m_data->m_gripperRigidbodyFixed->setPivotInB(gVRGripperPos); + if (m_data->m_gripperMultiBody->getNumLinks() > 2) + { + for (int i = 0; i < 2; i++) + { + if (supportsJointMotor(m_data->m_gripperMultiBody, i * 2)) + { + btMultiBodyJointMotor* motor = (btMultiBodyJointMotor*)m_data->m_gripperMultiBody->getLink(i * 2).m_userPtr; + if (motor) + { + motor->setErp(0.005); + + if (gVRGripperClosed) + { + motor->setPositionTarget(0.2, 1); + } + else + { + motor->setPositionTarget(SIMD_HALF_PI, 1); + } + motor->setVelocityTarget(0, 0.1); + btScalar maxImp = 550.*m_data->m_physicsDeltaTime; + motor->setMaxAppliedImpulse(maxImp); + } + } + } + } } int maxSteps = 3; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 49f63a9a4..f84e7356c 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -18,6 +18,8 @@ extern btVector3 gLastPickPos; btVector3 gVRTeleportPos(0,0,0); +extern bool gVRGripperClosed; + bool gDebugRenderToggle = false; void MotionThreadFunc(void* userPtr,void* lsMemory); void* MotionlsMemoryFunc(); @@ -1116,20 +1118,29 @@ void PhysicsServerExample::vrControllerButtonCallback(int controllerId, int butt gDebugRenderToggle = !gDebugRenderToggle; } + if (button==1) { m_args[0].m_isVrControllerTeleporting[controllerId] = true; } - if ((button==33)) + if (controllerId == 3 && (button == 33)) { - m_args[0].m_isVrControllerPicking[controllerId] = (state!=0); - m_args[0].m_isVrControllerReleasing[controllerId] = (state==0); + gVRGripperClosed =state; } - if ((button==33)||(button==1)) + else { - m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); - m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); + + if ((button == 33)) + { + m_args[0].m_isVrControllerPicking[controllerId] = (state != 0); + m_args[0].m_isVrControllerReleasing[controllerId] = (state == 0); + } + if ((button == 33) || (button == 1)) + { + m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); + m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]); + } } } @@ -1144,13 +1155,17 @@ void PhysicsServerExample::vrControllerMoveCallback(int controllerId, float pos[ printf("Controller Id exceeds max: %d > %d", controllerId, MAX_VR_CONTROLLERS); return; } - m_args[0].m_vrControllerPos[controllerId].setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); - m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0],orn[1],orn[2],orn[3]); - - gVRGripperPos.setValue(pos[0]+gVRTeleportPos[0],pos[1]+gVRTeleportPos[1],pos[2]+gVRTeleportPos[2]); - 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); - + if (controllerId == 3) + { + gVRGripperPos.setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); + 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); + } + else + { + m_args[0].m_vrControllerPos[controllerId].setValue(pos[0] + gVRTeleportPos[0], pos[1] + gVRTeleportPos[1], pos[2] + gVRTeleportPos[2]); + m_args[0].m_vrControllerOrn[controllerId].setValue(orn[0], orn[1], orn[2], orn[3]); + } } B3_STANDALONE_EXAMPLE(PhysicsServerCreateFunc) diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index aaa453fce..d806ad27c 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -700,12 +700,16 @@ bool CMainApplication::HandleInput() if (button==2) { glPolygonMode( GL_FRONT_AND_BACK, GL_LINE ); - gDebugDrawFlags = btIDebugDraw::DBG_DrawContactPoints + ///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 //btIDebugDraw::DBG_DrawConstraintLimits+ //btIDebugDraw::DBG_DrawConstraints ; } + sExample->vrControllerButtonCallback(unDevice, button, 1, pos, orn); + } else { diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp index 326a6ac48..3762e17a5 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.cpp @@ -26,7 +26,8 @@ btMultiBodyJointMotor::btMultiBodyJointMotor(btMultiBody* body, int link, btScal m_desiredVelocity(desiredVelocity), m_desiredPosition(0), m_kd(1.), - m_kp(0) + m_kp(0), + m_erp(1) { m_maxAppliedImpulse = maxMotorImpulse; @@ -123,7 +124,7 @@ void btMultiBodyJointMotor::createConstraintRows(btMultiBodyConstraintArray& con int dof = 0; btScalar currentPosition = m_bodyA->getJointPosMultiDof(m_linkA)[dof]; btScalar currentVelocity = m_bodyA->getJointVelMultiDof(m_linkA)[dof]; - btScalar positionStabiliationTerm = (m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; + btScalar positionStabiliationTerm = m_erp*(m_desiredPosition-currentPosition)/infoGlobal.m_timeStep; btScalar velocityError = (m_desiredVelocity - currentVelocity); btScalar rhs = m_kp * positionStabiliationTerm + currentVelocity+m_kd * velocityError; diff --git a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h index a8bcf606c..ceac3b65e 100644 --- a/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h +++ b/src/BulletDynamics/Featherstone/btMultiBodyJointMotor.h @@ -29,6 +29,7 @@ protected: btScalar m_desiredPosition; btScalar m_kd; btScalar m_kp; + btScalar m_erp; public: @@ -57,6 +58,14 @@ public: m_kp = kp; } + virtual void setErp(btScalar erp) + { + m_erp = erp; + } + virtual btScalar getErp() const + { + return m_erp; + } virtual void debugDraw(class btIDebugDraw* drawer) {