From 63486a712c3d5571c387b61da6fc439282f45e26 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 16 Feb 2017 14:19:09 -0800 Subject: [PATCH 1/3] VR video recording, use command-line --mp4=videoname.mp4 tune gripper grasp example with tefal pan, 800Newton force. URDF importer: if using single transform 1 child shape, don't use compound shape. if renderGUI is false, don't intercept mouse clicks add manyspheres.py example (performance is pretty bad, will look into it) [pybullet] expose contactBreakingThreshold --- data/gripper/wsg50_one_motor_gripper_new.sdf | 8 +++ data/sphere_1cm.urdf | 14 +--- .../CommonInterfaces/CommonExampleInterface.h | 1 + .../InProcessExampleBrowser.cpp | 1 + .../ExampleBrowser/OpenGLExampleBrowser.cpp | 27 +++++-- .../ExampleBrowser/OpenGLExampleBrowser.h | 2 + .../Importers/ImportURDFDemo/URDF2Bullet.cpp | 11 ++- examples/OpenGLWindow/SimpleOpenGL3App.cpp | 2 +- .../RoboticsLearning/GripperGraspExample.cpp | 16 ++--- examples/SharedMemory/PhysicsClientC_API.cpp | 10 +++ examples/SharedMemory/PhysicsClientC_API.h | 2 +- .../PhysicsServerCommandProcessor.cpp | 8 ++- .../SharedMemory/PhysicsServerExample.cpp | 71 ++++++++++--------- examples/SharedMemory/SharedMemoryCommands.h | 4 +- .../SharedMemoryInProcessPhysicsC_API.cpp | 17 ++--- .../StandaloneMain/hellovr_opengl_main.cpp | 9 ++- examples/pybullet/manyspheres.py | 6 +- examples/pybullet/pybullet.c | 13 +++- 18 files changed, 140 insertions(+), 82 deletions(-) diff --git a/data/gripper/wsg50_one_motor_gripper_new.sdf b/data/gripper/wsg50_one_motor_gripper_new.sdf index 5bb80d6a6..54f469fe0 100644 --- a/data/gripper/wsg50_one_motor_gripper_new.sdf +++ b/data/gripper/wsg50_one_motor_gripper_new.sdf @@ -303,6 +303,10 @@ + + .3 + 0.04 + 0.062 0 0.145 0 0 1.5708 0.2 @@ -343,6 +347,10 @@ + + .3 + 0.04 + -0.062 0 0.145 0 0 4.71239 0.2 diff --git a/data/sphere_1cm.urdf b/data/sphere_1cm.urdf index b44069877..05a07cde7 100644 --- a/data/sphere_1cm.urdf +++ b/data/sphere_1cm.urdf @@ -2,23 +2,15 @@ - - + + - - - - - - - - - + diff --git a/examples/CommonInterfaces/CommonExampleInterface.h b/examples/CommonInterfaces/CommonExampleInterface.h index df5f1817e..fd0b2651a 100644 --- a/examples/CommonInterfaces/CommonExampleInterface.h +++ b/examples/CommonInterfaces/CommonExampleInterface.h @@ -37,6 +37,7 @@ public: virtual void initPhysics()=0; virtual void exitPhysics()=0; + virtual void updateGraphics(){} virtual void stepSimulation(float deltaTime)=0; virtual void renderScene()=0; virtual void physicsDebugDraw(int debugFlags)=0;//for now we reuse the flags in Bullet/src/LinearMath/btIDebugDraw.h diff --git a/examples/ExampleBrowser/InProcessExampleBrowser.cpp b/examples/ExampleBrowser/InProcessExampleBrowser.cpp index 0b979b361..88a2abd99 100644 --- a/examples/ExampleBrowser/InProcessExampleBrowser.cpp +++ b/examples/ExampleBrowser/InProcessExampleBrowser.cpp @@ -266,6 +266,7 @@ void ExampleBrowserThreadFunc(void* userPtr,void* lsMemory) { B3_PROFILE("clock.usleep"); clock.usleep(gMinUpdateTimeMicroSecs/10.); + exampleBrowser->updateGraphics(); } else { B3_PROFILE("exampleBrowser->update"); diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index fb13df898..65464268c 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -309,8 +309,11 @@ static void MyMouseMoveCallback( float x, float y) bool handled = false; if (sCurrentDemo) handled = sCurrentDemo->mouseMoveCallback(x,y); - if (!handled && gui2) - handled = gui2->mouseMoveCallback(x,y); + if (renderGui) + { + if (!handled && gui2) + handled = gui2->mouseMoveCallback(x,y); + } if (!handled) { if (prevMouseMoveCallback) @@ -327,9 +330,11 @@ static void MyMouseButtonCallback(int button, int state, float x, float y) if (sCurrentDemo) handled = sCurrentDemo->mouseButtonCallback(button,state,x,y); - if (!handled && gui2) - handled = gui2->mouseButtonCallback(button,state,x,y); - + if (renderGui) + { + if (!handled && gui2) + handled = gui2->mouseButtonCallback(button,state,x,y); + } if (!handled) { if (prevMouseButtonCallback ) @@ -1125,6 +1130,18 @@ bool OpenGLExampleBrowser::requestedExit() return s_window->requestedExit(); } +void OpenGLExampleBrowser::updateGraphics() +{ + if (sCurrentDemo) + { + if (!pauseSimulation || singleStepSimulation) + { + B3_PROFILE("sCurrentDemo->updateGraphics"); + sCurrentDemo->updateGraphics(); + } + } +} + void OpenGLExampleBrowser::update(float deltaTime) { b3ChromeUtilsEnableProfiling(); diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.h b/examples/ExampleBrowser/OpenGLExampleBrowser.h index 1f68abedb..533b666ff 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.h +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.h @@ -19,6 +19,8 @@ public: virtual void update(float deltaTime); + virtual void updateGraphics(); + virtual bool requestedExit(); virtual void setSharedMemoryInterface(class SharedMemoryInterface* sharedMem); diff --git a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp index 30b2d90f4..30f35963a 100644 --- a/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp +++ b/examples/Importers/ImportURDFDemo/URDF2Bullet.cpp @@ -71,7 +71,7 @@ struct URDF2BulletCachedData } - void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) + void registerMultiBody( int urdfLinkIndex, class btMultiBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame) { m_urdfLinkLocalInertialFrames[urdfLinkIndex] = localInertialFrame; } @@ -81,7 +81,7 @@ struct URDF2BulletCachedData return m_urdfLink2rigidBodies[urdfLinkIndex]; } - void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCompoundShape* compound, const btTransform& localInertialFrame) + void registerRigidBody( int urdfLinkIndex, class btRigidBody* body, const btTransform& worldTransform, btScalar mass, const btVector3& localInertiaDiagonal, const class btCollisionShape* compound, const btTransform& localInertialFrame) { btAssert(m_urdfLink2rigidBodies[urdfLinkIndex]==0); @@ -250,7 +250,12 @@ void ConvertURDF2BulletInternal( - btCompoundShape* compoundShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); + btCompoundShape* tmpShape = u2b.convertLinkCollisionShapes(urdfLinkIndex,pathPrefix,localInertialFrame); + btCollisionShape* compoundShape = tmpShape; + if (tmpShape->getNumChildShapes() == 1 && tmpShape->getChildTransform(0)==btTransform::getIdentity()) + { + compoundShape = tmpShape->getChildShape(0); + } int graphicsIndex = u2b.convertLinkVisualShapes(urdfLinkIndex,pathPrefix,localInertialFrame); diff --git a/examples/OpenGLWindow/SimpleOpenGL3App.cpp b/examples/OpenGLWindow/SimpleOpenGL3App.cpp index e7c51fe15..212d80a6f 100644 --- a/examples/OpenGLWindow/SimpleOpenGL3App.cpp +++ b/examples/OpenGLWindow/SimpleOpenGL3App.cpp @@ -787,7 +787,7 @@ void SimpleOpenGL3App::dumpFramesToVideo(const char* mp4FileName) #ifdef _WIN32 sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " - "-threads 0 -y -b 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName); + "-threads 0 -y -b:v 50000k -t 20 -c:v libx264 -preset slow -crf 22 -an -pix_fmt yuv420p -vf vflip %s", width, height, mp4FileName); //sprintf(cmd, "ffmpeg -r 60 -f rawvideo -pix_fmt rgba -s %dx%d -i - " // "-y -crf 0 -b:v 1500000 -an -vcodec h264 -vf vflip %s", width, height, mp4FileName); diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 4b86a567d..5dbbd1562 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -16,8 +16,8 @@ #include "b3RobotSimAPI.h" #include "../Utils/b3Clock.h" -static btScalar sGripperVerticalVelocity = -0.2f; -static btScalar sGripperClosingTargetVelocity = 0.5f; +static btScalar sGripperVerticalVelocity = 0.f; +static btScalar sGripperClosingTargetVelocity = -0.7f; class GripperGraspExample : public CommonExampleInterface { @@ -226,9 +226,9 @@ public: { b3RobotSimLoadFileArgs args(""); b3RobotSimLoadFileResults results; - args.m_fileName = "sphere_small.urdf"; - args.m_startPosition.setValue(0, 0, .107); - args.m_startOrientation.setEulerZYX(0, 0, 0); + args.m_fileName = "dinnerware/pan_tefal.urdf"; + args.m_startPosition.setValue(0, -0.2, .47); + args.m_startOrientation.setEulerZYX(SIMD_HALF_PI, 0, 0); args.m_useMultiBody = true; m_robotSim.loadFile(args, results); } @@ -492,7 +492,7 @@ public: int fingerJointIndices[2]={0,1}; double fingerTargetVelocities[2]={sGripperVerticalVelocity,sGripperClosingTargetVelocity }; - double maxTorqueValues[2]={50.0,50.0}; + double maxTorqueValues[2]={800.0,800.0}; for (int i=0;i<2;i++) { b3JointMotorArgs controlArgs(CONTROL_MODE_VELOCITY); @@ -550,8 +550,8 @@ public: virtual void resetCamera() { float dist = 1.5; - float pitch = 12; - float yaw = -10; + float pitch = 18; + float yaw = 10; float targetPos[3]={-0.2,0.8,0.3}; if (m_app->m_renderer && m_app->m_renderer->getActiveCamera()) { diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index f02e332a4..e311cff08 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -338,6 +338,16 @@ int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandl return 0; } +int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command->m_type == CMD_SEND_PHYSICS_SIMULATION_PARAMETERS); + + command->m_physSimParamArgs.m_contactBreakingThreshold = contactBreakingThreshold; + command->m_updateFlags |= SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD; + return 0; +} + int b3PhysicsParamSetNumSolverIterations(b3SharedMemoryCommandHandle commandHandle, int numSolverIterations) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index febe08396..b150e54d7 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -191,7 +191,7 @@ int b3PhysicsParamSetCollisionFilterMode(b3SharedMemoryCommandHandle commandHand int b3PhysicsParamSetUseSplitImpulse(b3SharedMemoryCommandHandle commandHandle, int useSplitImpulse); int b3PhysicsParamSetSplitImpulsePenetrationThreshold(b3SharedMemoryCommandHandle commandHandle, double splitImpulsePenetrationThreshold); - +int b3PhysicsParamSetContactBreakingThreshold(b3SharedMemoryCommandHandle commandHandle, double contactBreakingThreshold); //b3PhysicsParamSetInternalSimFlags is for internal/temporary/easter-egg/experimental demo purposes //Use at own risk: magic things may or my not happen when calling this API diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index cd4260469..75271eb37 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -12,6 +12,7 @@ #include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h" #include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h" +#include "BulletCollision/NarrowPhaseCollision/btPersistentManifold.h" #include "LinearMath/btHashMap.h" #include "BulletInverseDynamics/MultiBodyTree.hpp" #include "IKTrajectoryHelper.h" @@ -798,7 +799,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; - m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 150; m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7; // m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2; //todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp) @@ -2724,7 +2725,10 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = clientCmd.m_physSimParamArgs.m_numSolverIterations; } - + if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD) + { + gContactBreakingThreshold = clientCmd.m_physSimParamArgs.m_contactBreakingThreshold; + } if (clientCmd.m_updateFlags&SIM_PARAM_UPDATE_COLLISION_FILTER_MODE) { m_data->m_broadphaseCollisionFilterCallback->m_filterMode = clientCmd.m_physSimParamArgs.m_collisionFilterMode; diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 4c9ae62fe..520a6d904 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -553,7 +553,7 @@ public: while (m_cs->getSharedParam(1)!=eGUIHelperIdle) { - b3Clock::usleep(100); + b3Clock::usleep(0); } } @@ -940,6 +940,8 @@ public: virtual void stepSimulation(float deltaTime); + virtual void updateGraphics(); + void enableCommandLogging() { m_physicsServer.enableCommandLogging(true,"BulletPhysicsCommandLog.bin"); @@ -1342,39 +1344,8 @@ bool PhysicsServerExample::wantsTermination() return m_wantsShutdown; } - - -void PhysicsServerExample::stepSimulation(float deltaTime) +void PhysicsServerExample::updateGraphics() { - BT_PROFILE("PhysicsServerExample::stepSimulation"); - - //this->m_physicsServer.processClientCommands(); - - for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--) - { - if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime) - { - m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime; - if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0) - { - m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1); - m_multiThreadedHelper->m_userDebugLines.pop_back(); - } - } - } - - for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--) - { - if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime) - { - m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime; - if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0) - { - m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1); - m_multiThreadedHelper->m_userDebugText.pop_back(); - } - } - } //check if any graphics related tasks are requested switch (m_multiThreadedHelper->getCriticalSection()->getSharedParam(1)) @@ -1543,6 +1514,40 @@ void PhysicsServerExample::stepSimulation(float deltaTime) } +} + +void PhysicsServerExample::stepSimulation(float deltaTime) +{ + BT_PROFILE("PhysicsServerExample::stepSimulation"); + + //this->m_physicsServer.processClientCommands(); + + for (int i = m_multiThreadedHelper->m_userDebugLines.size()-1;i>=0;i--) + { + if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime) + { + m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime -= deltaTime; + if (m_multiThreadedHelper->m_userDebugLines[i].m_lifeTime<=0) + { + m_multiThreadedHelper->m_userDebugLines.swap(i,m_multiThreadedHelper->m_userDebugLines.size()-1); + m_multiThreadedHelper->m_userDebugLines.pop_back(); + } + } + } + + for (int i = m_multiThreadedHelper->m_userDebugText.size()-1;i>=0;i--) + { + if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime) + { + m_multiThreadedHelper->m_userDebugText[i].m_lifeTime -= deltaTime; + if (m_multiThreadedHelper->m_userDebugText[i].m_lifeTime<=0) + { + m_multiThreadedHelper->m_userDebugText.swap(i,m_multiThreadedHelper->m_userDebugText.size()-1); + m_multiThreadedHelper->m_userDebugText.pop_back(); + } + } + } + updateGraphics(); diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 847a0de75..94d8b6a9c 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -313,7 +313,8 @@ enum EnumSimParamUpdateFlags SIM_PARAM_UPDATE_INTERNAL_SIMULATION_FLAGS=64, SIM_PARAM_UPDATE_USE_SPLIT_IMPULSE=128, SIM_PARAM_UPDATE_SPLIT_IMPULSE_PENETRATION_THRESHOLD = 256, - SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512 + SIM_PARAM_UPDATE_COLLISION_FILTER_MODE=512, + SIM_PARAM_UPDATE_CONTACT_BREAKING_THRESHOLD = 1024, }; enum EnumLoadBunnyUpdateFlags @@ -340,6 +341,7 @@ struct SendPhysicsSimulationParameters bool m_allowRealTimeSimulation; int m_useSplitImpulse; double m_splitImpulsePenetrationThreshold; + double m_contactBreakingThreshold; int m_internalSimFlags; double m_defaultContactERP; int m_collisionFilterMode; diff --git a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp index b4933aeb4..f1a4b4966 100644 --- a/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp +++ b/examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp @@ -44,16 +44,13 @@ public: PhysicsClientSharedMemory::disconnectSharedMemory(); } unsigned long int ms = m_clock.getTimeMilliseconds(); - if (ms>20) - { - m_clock.reset(); - btUpdateInProcessExampleBrowserMainThread(m_data); - } else - { - //b3Clock::usleep(100); - } - return PhysicsClientSharedMemory::processServerStatus(); - + if (ms>20) + { + m_clock.reset(); + btUpdateInProcessExampleBrowserMainThread(m_data); + } + b3Clock::usleep(0); + return PhysicsClientSharedMemory::processServerStatus(); } diff --git a/examples/StandaloneMain/hellovr_opengl_main.cpp b/examples/StandaloneMain/hellovr_opengl_main.cpp index 590312503..4c6b16ffe 100644 --- a/examples/StandaloneMain/hellovr_opengl_main.cpp +++ b/examples/StandaloneMain/hellovr_opengl_main.cpp @@ -135,6 +135,8 @@ public: void SetupRenderModelForTrackedDevice( vr::TrackedDeviceIndex_t unTrackedDeviceIndex ); CGLRenderModel *FindOrLoadRenderModel( const char *pchRenderModelName ); + SimpleOpenGL3App* getApp() { return m_app;} + private: bool m_bDebugOpenGL; bool m_bVerbose; @@ -2260,7 +2262,7 @@ int main(int argc, char *argv[]) b3ChromeUtilsEnableProfiling(); } - + #ifdef BT_USE_CUSTOM_PROFILER b3SetCustomEnterProfileZoneFunc(dcEnter); b3SetCustomLeaveProfileZoneFunc(dcLeave); @@ -2287,6 +2289,11 @@ int main(int argc, char *argv[]) } + char* gVideoFileName = 0; + args.GetCmdLineArgument("mp4",gVideoFileName); + if (gVideoFileName) + pMainApplication->getApp()->dumpFramesToVideo(gVideoFileName); + //request disable VSYNC typedef bool (APIENTRY *PFNWGLSWAPINTERVALFARPROC)(int); PFNWGLSWAPINTERVALFARPROC wglSwapIntervalEXT = 0; diff --git a/examples/pybullet/manyspheres.py b/examples/pybullet/manyspheres.py index c06a477ab..1c4c37b97 100644 --- a/examples/pybullet/manyspheres.py +++ b/examples/pybullet/manyspheres.py @@ -7,9 +7,9 @@ p.loadURDF("tray/traybox.urdf",useMaximalCoordinates=True) gravXid = p.addUserDebugParameter("gravityX",-10,10,0) gravYid = p.addUserDebugParameter("gravityY",-10,10,0) -gravZid = p.addUserDebugParameter("gravityZ",-10,10,0) +gravZid = p.addUserDebugParameter("gravityZ",-10,10,-10) p.setPhysicsEngineParameter(numSolverIterations=10) - +p.setPhysicsEngineParameter(contactBreakingThreshold=0.001) for i in range (10): for j in range (10): for k in range (5): @@ -21,5 +21,5 @@ while True: gravY = p.readUserDebugParameter(gravYid) gravZ = p.readUserDebugParameter(gravZid) p.setGravity(gravX,gravY,gravZ) - time.sleep(1) + time.sleep(0.01) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 65cc7e108..5b43cf6d7 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -583,14 +583,15 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar double splitImpulsePenetrationThreshold = -1; int numSubSteps = -1; int collisionFilterMode = -1; + double contactBreakingThreshold = -1; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "physicsClientId", NULL }; + static char *kwlist[] = { "fixedTimeStep", "numSolverIterations","useSplitImpulse","splitImpulsePenetrationThreshold", "numSubSteps","collisionFilterMode", "contactBreakingThreshold", "physicsClientId", NULL }; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiii", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps, - &collisionFilterMode, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "|diidiidi", kwlist,&fixedTimeStep,&numSolverIterations,&useSplitImpulse,&splitImpulsePenetrationThreshold,&numSubSteps, + &collisionFilterMode, &contactBreakingThreshold, &physicsClientId)) { return NULL; } @@ -610,6 +611,7 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetNumSolverIterations(command, numSolverIterations); } + if (collisionFilterMode>=0) { b3PhysicsParamSetCollisionFilterMode(command, collisionFilterMode); @@ -630,6 +632,11 @@ static PyObject* pybullet_setPhysicsEngineParameter(PyObject* self, PyObject* ar { b3PhysicsParamSetSplitImpulsePenetrationThreshold(command, splitImpulsePenetrationThreshold); } + if (contactBreakingThreshold>=0) + { + b3PhysicsParamSetContactBreakingThreshold(command,contactBreakingThreshold); + } + //ret = b3PhysicsParamSetRealTimeSimulation(command, enableRealTimeSimulation); From 65deeee64b2663c907cd9e7b8411b535d96b9e0f Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 16 Feb 2017 14:29:51 -0800 Subject: [PATCH 2/3] add optimized tray/traybox.urdf --- data/tray/tray_textured.mtl | 13 +++ data/tray/tray_textured.obj | 213 ++++++++++++++++++++++++++++++++++++ data/tray/traybox.urdf | 49 +++++++++ 3 files changed, 275 insertions(+) create mode 100644 data/tray/tray_textured.mtl create mode 100644 data/tray/tray_textured.obj create mode 100644 data/tray/traybox.urdf diff --git a/data/tray/tray_textured.mtl b/data/tray/tray_textured.mtl new file mode 100644 index 000000000..e3b14fbaf --- /dev/null +++ b/data/tray/tray_textured.mtl @@ -0,0 +1,13 @@ +# Blender MTL File: 'None' +# Material Count: 1 + +newmtl None +Ns 0.000000 +Ka 0.000000 0.000000 0.000000 +Kd 0.800000 0.800000 0.800000 +Ks 0.800000 0.800000 0.800000 +Ke 0.000000 0.000000 0.000000 +Ni 1.000000 +d 1.000000 +illum 2 +map_Kd tray.jpg diff --git a/data/tray/tray_textured.obj b/data/tray/tray_textured.obj new file mode 100644 index 000000000..f3523df4d --- /dev/null +++ b/data/tray/tray_textured.obj @@ -0,0 +1,213 @@ +# Blender v2.78 (sub 0) OBJ File: '' +# www.blender.org +mtllib tray_textured.mtl +o edge_4_Cube.001 +v -0.573309 0.580000 0.261247 +v -0.426691 0.419400 -0.002214 +v -0.590083 0.580000 0.250354 +v -0.573309 -0.580000 0.261247 +v -0.409917 0.419400 0.008679 +v -0.590083 -0.580000 0.250354 +v -0.409917 -0.419400 0.009162 +v -0.426691 -0.419400 -0.001731 +vt 0.9046 0.2397 +vt 0.7929 0.2434 +vt 0.9174 0.2393 +vt 0.9537 0.7559 +vt 0.7801 0.2438 +vt 0.9664 0.7554 +vt 0.8291 0.7599 +vt 0.8419 0.7595 +vn 0.2565 0.8821 -0.3950 +vn 0.8392 0.0002 0.5438 +vn 0.8395 0.0000 0.5433 +vn 0.8396 -0.0000 0.5432 +vn 0.2568 -0.8819 -0.3954 +vn -0.8396 -0.0002 -0.5433 +vn -0.8392 -0.0000 -0.5438 +vn -0.8391 0.0000 -0.5439 +vn 0.5446 -0.0005 -0.8387 +vn -0.5446 -0.0000 0.8387 +vn 0.8391 0.0003 0.5440 +vn -0.8397 -0.0003 -0.5430 +usemtl None +s 1 +f 1/1/1 2/2/1 3/3/1 +f 4/4/2 5/5/3 1/1/4 +f 6/6/5 7/7/5 4/4/5 +f 3/3/6 8/8/7 6/6/8 +f 5/5/9 8/8/9 2/2/9 +f 4/4/10 3/3/10 6/6/10 +f 1/1/1 5/5/1 2/2/1 +f 4/4/2 7/7/11 5/5/3 +f 6/6/5 8/8/5 7/7/5 +f 3/3/6 2/2/12 8/8/7 +f 5/5/9 7/7/9 8/8/9 +f 4/4/10 1/1/10 3/3/10 +o edge_1_Cube.003 +v 0.580000 0.590083 0.250354 +v -0.419960 0.426691 -0.001860 +v -0.580000 0.590083 0.250354 +v 0.580000 0.573309 0.261247 +v 0.420014 0.426691 -0.001059 +v -0.580000 0.573309 0.261247 +v 0.420014 0.409917 0.009834 +v -0.419960 0.409917 0.009033 +vt 0.8346 0.9187 +vt 0.2203 0.8574 +vt 0.1480 0.9187 +vt 0.8346 0.9129 +vt 0.7623 0.8574 +vt 0.1480 0.9129 +vt 0.7623 0.8511 +vt 0.2203 0.8511 +vn 0.0004 0.8386 -0.5448 +vn 0.0001 0.8391 -0.5439 +vn 0.0000 0.8393 -0.5437 +vn 0.8823 -0.2564 -0.3948 +vn -0.0004 -0.8392 0.5439 +vn -0.0001 -0.8386 0.5447 +vn 0.0000 -0.8385 0.5449 +vn -0.8826 -0.2560 -0.3942 +vn 0.0008 -0.5446 -0.8387 +vn 0.0000 0.5446 0.8387 +vn 0.0005 0.8383 -0.5452 +vn -0.0005 -0.8394 0.5435 +usemtl None +s 1 +f 9/9/13 10/10/14 11/11/15 +f 12/12/16 13/13/16 9/9/16 +f 14/14/17 15/15/18 12/12/19 +f 11/11/20 16/16/20 14/14/20 +f 13/13/21 16/16/21 10/10/21 +f 12/12/22 11/11/22 14/14/22 +f 9/9/13 13/13/23 10/10/14 +f 12/12/16 15/15/16 13/13/16 +f 14/14/17 16/16/24 15/15/18 +f 11/11/20 10/10/20 16/16/20 +f 13/13/21 15/15/21 16/16/21 +f 12/12/22 9/9/22 11/11/22 +o edge_2_Cube +v 0.590083 0.580000 0.250354 +v 0.409917 0.420060 0.009390 +v 0.573309 0.580000 0.261247 +v 0.590083 -0.580000 0.250354 +v 0.426691 0.420060 -0.001503 +v 0.573309 -0.580000 0.261247 +v 0.426691 -0.419158 -0.002053 +v 0.409917 -0.419158 0.008840 +vt 0.9410 0.8520 +vt 0.7523 0.8566 +vt 0.9234 0.8524 +vt 0.8896 0.1426 +vt 0.7698 0.8562 +vt 0.8721 0.1430 +vt 0.7185 0.1468 +vt 0.7009 0.1472 +vn -0.2561 0.8826 -0.3943 +vn 0.8394 0.0003 -0.5435 +vn 0.8390 0.0001 -0.5441 +vn 0.8389 0.0000 -0.5443 +vn -0.2569 -0.8818 -0.3956 +vn -0.8390 -0.0003 0.5441 +vn -0.8394 -0.0001 0.5436 +vn -0.8395 -0.0000 0.5434 +vn -0.5446 0.0005 -0.8387 +vn 0.5446 -0.0000 0.8387 +vn 0.8396 0.0004 -0.5433 +vn -0.8388 -0.0004 0.5444 +usemtl None +s 1 +f 17/17/25 18/18/25 19/19/25 +f 20/20/26 21/21/27 17/17/28 +f 22/22/29 23/23/29 20/20/29 +f 19/19/30 24/24/31 22/22/32 +f 21/21/33 24/24/33 18/18/33 +f 20/20/34 19/19/34 22/22/34 +f 17/17/25 21/21/25 18/18/25 +f 20/20/26 23/23/35 21/21/27 +f 22/22/29 24/24/29 23/23/29 +f 19/19/30 18/18/36 24/24/31 +f 21/21/33 23/23/33 24/24/33 +f 20/20/34 17/17/34 19/19/34 +o base_Cube.004 +v 0.420000 0.420000 0.010000 +v -0.420000 0.420000 -0.010000 +v -0.420000 0.420000 0.010000 +v 0.420000 -0.420000 0.010000 +v 0.420000 0.420000 -0.010000 +v -0.420000 -0.420000 0.010000 +v 0.420000 -0.420000 -0.010000 +v -0.420000 -0.420000 -0.010000 +vt 0.7524 0.8072 +vt -0.3038 0.8371 +vt -0.3038 0.8371 +vt 0.7012 0.1905 +vt 0.7524 0.8072 +vt -0.3550 0.2204 +vt 0.7012 0.1905 +vt -0.3550 0.2204 +vn 0.0000 1.0000 0.0000 +vn 1.0000 -0.0000 0.0000 +vn 0.0000 -1.0000 0.0000 +vn -1.0000 -0.0000 0.0000 +vn 0.0000 0.0000 -1.0000 +vn 0.0000 -0.0000 1.0000 +usemtl None +s 1 +f 25/25/37 26/26/37 27/27/37 +f 28/28/38 29/29/38 25/25/38 +f 30/30/39 31/31/39 28/28/39 +f 27/27/40 32/32/40 30/30/40 +f 29/29/41 32/32/41 26/26/41 +f 28/28/42 27/27/42 30/30/42 +f 25/25/37 29/29/37 26/26/37 +f 28/28/38 31/31/38 29/29/38 +f 30/30/39 32/32/39 31/31/39 +f 27/27/40 26/26/40 32/32/40 +f 29/29/41 31/31/41 32/32/41 +f 28/28/42 25/25/42 27/27/42 +o edge_3_Cube.002 +v 0.580000 -0.573309 0.261247 +v -0.419400 -0.409917 0.008678 +v -0.580000 -0.573309 0.261247 +v 0.580000 -0.590083 0.250354 +v 0.419883 -0.409917 0.009162 +v -0.580000 -0.590083 0.250354 +v 0.419883 -0.426691 -0.001731 +v -0.419400 -0.426691 -0.002215 +vt 0.8690 0.1040 +vt 0.1365 0.1739 +vt 0.0188 0.1040 +vt 0.8690 0.0968 +vt 0.7517 0.1739 +vt 0.0188 0.0968 +vt 0.7517 0.1668 +vt 0.1365 0.1668 +vn -0.0002 0.8392 0.5438 +vn -0.0000 0.8395 0.5433 +vn 0.0000 0.8396 0.5432 +vn 0.8825 0.2562 -0.3945 +vn 0.0002 -0.8396 -0.5433 +vn 0.0000 -0.8392 -0.5438 +vn 0.0000 -0.8391 -0.5439 +vn -0.8821 0.2565 -0.3950 +vn -0.8822 0.2565 -0.3950 +vn 0.0005 0.5446 -0.8387 +vn 0.0000 -0.5446 0.8387 +vn -0.0003 0.8391 0.5440 +vn 0.0003 -0.8397 -0.5430 +usemtl None +s 1 +f 33/33/43 34/34/44 35/35/45 +f 36/36/46 37/37/46 33/33/46 +f 38/38/47 39/39/48 36/36/49 +f 35/35/50 40/40/51 38/38/51 +f 37/37/52 40/40/52 34/34/52 +f 36/36/53 35/35/53 38/38/53 +f 33/33/43 37/37/54 34/34/44 +f 36/36/46 39/39/46 37/37/46 +f 38/38/47 40/40/55 39/39/48 +f 35/35/50 34/34/51 40/40/51 +f 37/37/52 39/39/52 40/40/52 +f 36/36/53 33/33/53 35/35/53 diff --git a/data/tray/traybox.urdf b/data/tray/traybox.urdf new file mode 100644 index 000000000..58e578f11 --- /dev/null +++ b/data/tray/traybox.urdf @@ -0,0 +1,49 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 8f546acbe863008f93f25d603aa040317c151916 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Thu, 16 Feb 2017 15:56:42 -0800 Subject: [PATCH 3/3] disable keyboard toggle in Linux/X11 revert num solver iterations to 50 (from 150) set solver iterations for one gripper grasp to 150 --- examples/ExampleBrowser/OpenGLExampleBrowser.cpp | 1 - examples/OpenGLWindow/X11OpenGLWindow.cpp | 7 ++++--- examples/RoboticsLearning/GripperGraspExample.cpp | 1 + examples/RoboticsLearning/b3RobotSimAPI.cpp | 10 ++++++++++ examples/RoboticsLearning/b3RobotSimAPI.h | 1 + .../SharedMemory/PhysicsServerCommandProcessor.cpp | 2 +- 6 files changed, 17 insertions(+), 5 deletions(-) diff --git a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp index 65464268c..9ffce6c96 100644 --- a/examples/ExampleBrowser/OpenGLExampleBrowser.cpp +++ b/examples/ExampleBrowser/OpenGLExampleBrowser.cpp @@ -257,7 +257,6 @@ void MyKeyboardCallback(int key, int state) } else { - b3ChromeUtilsStopTimingsAndWriteJsonFile(); } #endif //BT_NO_PROFILE diff --git a/examples/OpenGLWindow/X11OpenGLWindow.cpp b/examples/OpenGLWindow/X11OpenGLWindow.cpp index 789be7803..2e4e27cc3 100644 --- a/examples/OpenGLWindow/X11OpenGLWindow.cpp +++ b/examples/OpenGLWindow/X11OpenGLWindow.cpp @@ -839,7 +839,7 @@ void X11OpenGLWindow::pumpMessage() if (m_data->m_keyboardCallback) { -#if 0 +#if 1 unsigned short is_retriggered = 0; ///filter out keyboard repeat //see http://stackoverflow.com/questions/2100654/ignore-auto-repeat-in-x11-applications @@ -851,8 +851,8 @@ void X11OpenGLWindow::pumpMessage() if (nev.type == KeyPress && nev.xkey.time == m_data->m_xev.xkey.time && nev.xkey.keycode == m_data->m_xev.xkey.keycode) { - fprintf (stdout, "key #%ld was retriggered.\n", - (long) MyXLookupKeysym(&nev.xkey, 0)); + //fprintf (stdout, "key #%ld was retriggered.\n", + // (long) MyXLookupKeysym(&nev.xkey, 0)); // delete retriggered KeyPress event MyXNextEvent(m_data->m_dpy, & m_data->m_xev); @@ -861,6 +861,7 @@ void X11OpenGLWindow::pumpMessage() } #endif int state = 0; + if (!is_retriggered) (*m_data->m_keyboardCallback)(keycode,state); } diff --git a/examples/RoboticsLearning/GripperGraspExample.cpp b/examples/RoboticsLearning/GripperGraspExample.cpp index 5dbbd1562..60b4d3e4e 100644 --- a/examples/RoboticsLearning/GripperGraspExample.cpp +++ b/examples/RoboticsLearning/GripperGraspExample.cpp @@ -209,6 +209,7 @@ public: if ((m_options & eONE_MOTOR_GRASP)!=0) { + m_robotSim.setNumSolverIterations(150); { SliderParams slider("Vertical velocity",&sGripperVerticalVelocity); slider.m_minVal=-2; diff --git a/examples/RoboticsLearning/b3RobotSimAPI.cpp b/examples/RoboticsLearning/b3RobotSimAPI.cpp index 555ff13cc..3b8948761 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.cpp +++ b/examples/RoboticsLearning/b3RobotSimAPI.cpp @@ -478,6 +478,16 @@ void b3RobotSimAPI::setGravity(const b3Vector3& gravityAcceleration) } +void b3RobotSimAPI::setNumSolverIterations(int numIterations) +{ + b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); + b3SharedMemoryStatusHandle statusHandle; + b3PhysicsParamSetNumSolverIterations(command, numIterations); + statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClient, command); + b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED); + +} + void b3RobotSimAPI::setNumSimulationSubSteps(int numSubSteps) { b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClient); diff --git a/examples/RoboticsLearning/b3RobotSimAPI.h b/examples/RoboticsLearning/b3RobotSimAPI.h index 227053379..7e196e1b6 100644 --- a/examples/RoboticsLearning/b3RobotSimAPI.h +++ b/examples/RoboticsLearning/b3RobotSimAPI.h @@ -157,6 +157,7 @@ public: void setGravity(const b3Vector3& gravityAcceleration); void setNumSimulationSubSteps(int numSubSteps); + void setNumSolverIterations(int numIterations); bool calculateInverseKinematics(const struct b3RobotSimInverseKinematicArgs& args, struct b3RobotSimInverseKinematicsResults& results); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 75271eb37..9a6a76d75 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -799,7 +799,7 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld() m_data->m_dynamicsWorld->getSolverInfo().m_erp2 = 0.08; m_data->m_dynamicsWorld->getSolverInfo().m_linearSlop = 0.00001; - m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 150; + m_data->m_dynamicsWorld->getSolverInfo().m_numIterations = 50; m_data->m_dynamicsWorld->getSolverInfo().m_leastSquaresResidualThreshold = 1e-7; // m_data->m_dynamicsWorld->getSolverInfo().m_minimumSolverBatchSize = 2; //todo: islands/constraints are buggy in btMultiBodyDynamicsWorld! (performance + see slipping grasp)