From bb1da5b5f50fce595bc44d4054b9ad7d5fad5ff8 Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Mon, 31 Aug 2020 13:35:03 -0700 Subject: try to fix travis, update inertia for Laikago, reset camera at the right time (#3021) * laikago_toes_zup.urdf: fill in inertia values, computed as PyBullet does internally (from mass and collision volumes) * reset camera in the rendering thread to avoid artifacts * reset camera in the rendering thread to avoid artifacts * try to fix travis error --- .ci/script.sh | 2 +- examples/SharedMemory/PhysicsServerExample.cpp | 34 ++++++++++++++++++++-- .../pybullet_data/laikago/laikago_toes_zup.urdf | 26 ++++++++--------- 3 files changed, 46 insertions(+), 16 deletions(-) diff --git a/.ci/script.sh b/.ci/script.sh index 7fff323bf..f213df283 100755 --- a/.ci/script.sh +++ b/.ci/script.sh @@ -9,7 +9,7 @@ if [[ "$TRAVIS_OS_NAME" == "linux" && "$CXX" = "g++" ]]; then $SUDO apt-get install -y python3-pip $SUDO pip3 install -U wheel $SUDO pip3 install -U setuptools - $SUDO python3 setup.py install + python3 setup.py install --user python3 examples/pybullet/unittests/unittests.py --verbose python3 examples/pybullet/unittests/userDataTest.py --verbose python3 examples/pybullet/unittests/saveRestoreStateTest.py --verbose diff --git a/examples/SharedMemory/PhysicsServerExample.cpp b/examples/SharedMemory/PhysicsServerExample.cpp index 3a159f581..264ff7542 100644 --- a/examples/SharedMemory/PhysicsServerExample.cpp +++ b/examples/SharedMemory/PhysicsServerExample.cpp @@ -130,6 +130,7 @@ enum MultiThreadedGUIHelperCommunicationEnums eGUIHelperUpdateShape, eGUIHelperChangeGraphicsInstanceScaling, eGUIUserDebugRemoveAllParameters, + eGUIHelperResetCamera, }; #include @@ -1086,9 +1087,25 @@ public: { m_childGuiHelper->setUpAxis(axis); } + float m_resetCameraCamDist; + float m_resetCameraYaw; + float m_resetCameraPitch; + float m_resetCameraCamPosX; + float m_resetCameraCamPosY; + float m_resetCameraCamPosZ; + virtual void resetCamera(float camDist, float yaw, float pitch, float camPosX, float camPosY, float camPosZ) { - m_childGuiHelper->resetCamera(camDist, yaw, pitch, camPosX, camPosY, camPosZ); + m_cs->lock(); + m_resetCameraCamDist = camDist; + m_resetCameraYaw = yaw; + m_resetCameraPitch = pitch; + m_resetCameraCamPosX = camPosX; + m_resetCameraCamPosY = camPosY; + m_resetCameraCamPosZ = camPosZ; + m_cs->setSharedParam(1, eGUIHelperResetCamera); + workerThreadWait(); + //m_childGuiHelper->resetCamera(camDist, yaw, pitch, camPosX, camPosY, camPosZ); } virtual bool getCameraInfo(int* width, int* height, float viewMatrix[16], float projectionMatrix[16], float camUp[3], float camForward[3], float hor[3], float vert[3], float* yaw, float* pitch, float* camDist, float camTarget[3]) const @@ -1424,7 +1441,7 @@ public: float pitch = -35; float yaw = 50; float targetPos[3] = {0, 0, 0}; //-3,2.8,-2.5}; - m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); + m_multiThreadedHelper->m_childGuiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]); } virtual bool wantsTermination(); @@ -2360,6 +2377,19 @@ void PhysicsServerExample::updateGraphics() m_multiThreadedHelper->mainThreadRelease(); break; } + case eGUIHelperResetCamera: + { + m_multiThreadedHelper->m_childGuiHelper->resetCamera( + m_multiThreadedHelper->m_resetCameraCamDist, + m_multiThreadedHelper->m_resetCameraYaw, + m_multiThreadedHelper->m_resetCameraPitch, + m_multiThreadedHelper->m_resetCameraCamPosX, + m_multiThreadedHelper->m_resetCameraCamPosY, + m_multiThreadedHelper->m_resetCameraCamPosZ); + m_multiThreadedHelper->mainThreadRelease(); + break; + } + case eGUIHelperAutogenerateGraphicsObjects: { B3_PROFILE("eGUIHelperAutogenerateGraphicsObjects"); diff --git a/examples/pybullet/gym/pybullet_data/laikago/laikago_toes_zup.urdf b/examples/pybullet/gym/pybullet_data/laikago/laikago_toes_zup.urdf index d051affb8..8ebab9123 100644 --- a/examples/pybullet/gym/pybullet_data/laikago/laikago_toes_zup.urdf +++ b/examples/pybullet/gym/pybullet_data/laikago/laikago_toes_zup.urdf @@ -7,7 +7,7 @@ - + @@ -32,7 +32,7 @@ - + @@ -67,7 +67,7 @@ - + @@ -102,7 +102,7 @@ - + @@ -137,7 +137,7 @@ - + @@ -174,7 +174,7 @@ - + @@ -213,7 +213,7 @@ - + @@ -254,7 +254,7 @@ - + @@ -291,7 +291,7 @@ - + @@ -326,7 +326,7 @@ - + @@ -362,7 +362,7 @@ - + @@ -398,7 +398,7 @@ - + @@ -435,7 +435,7 @@ - + -- cgit v1.2.1