From 2cab56d6eff6e0fc2759bad5822bbdfffa98eb16 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 27 Jul 2018 15:48:24 +0200 Subject: [PATCH 1/8] prepare build system(s) for collisionFilterPlugin --- Extras/BulletRobotics/CMakeLists.txt | 1 + Extras/BulletRobotics/premake4.lua | 1 + examples/ExampleBrowser/CMakeLists.txt | 2 ++ examples/ExampleBrowser/premake4.lua | 1 + examples/SharedMemory/CMakeLists.txt | 2 ++ .../PhysicsServerCommandProcessor.cpp | 12 ++++++++++ .../collisionFilterPlugin.cpp | 24 +++++++++---------- examples/SharedMemory/premake4.lua | 2 +- examples/SharedMemory/tcp/premake4.lua | 1 + examples/SharedMemory/udp/premake4.lua | 1 + examples/TwoJoint/CMakeLists.txt | 1 + examples/Utils/ChromeTraceUtil.cpp | 7 +++--- examples/pybullet/CMakeLists.txt | 1 + setup.py | 1 + src/LinearMath/btQuickprof.h | 9 ++++--- test/SharedMemory/CMakeLists.txt | 1 + test/SharedMemory/premake4.lua | 3 +++ 17 files changed, 51 insertions(+), 19 deletions(-) diff --git a/Extras/BulletRobotics/CMakeLists.txt b/Extras/BulletRobotics/CMakeLists.txt index 0e1683360..ad37212a1 100644 --- a/Extras/BulletRobotics/CMakeLists.txt +++ b/Extras/BulletRobotics/CMakeLists.txt @@ -9,6 +9,7 @@ INCLUDE_DIRECTORIES( ) SET(BulletRobotics_SRCS + ../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp diff --git a/Extras/BulletRobotics/premake4.lua b/Extras/BulletRobotics/premake4.lua index b1e942ce5..deb4a869a 100644 --- a/Extras/BulletRobotics/premake4.lua +++ b/Extras/BulletRobotics/premake4.lua @@ -81,6 +81,7 @@ if not _OPTIONS["no-enet"] then files { + "../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp", diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 2677f2fe3..331bb1900 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -145,6 +145,8 @@ SET(BulletExampleBrowser_SRCS ../TinyRenderer/tgaimage.cpp ../TinyRenderer/our_gl.cpp ../TinyRenderer/TinyRenderer.cpp + ../SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp + ../SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.h ../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp ../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 1e5eea67a..1ed5e0965 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -115,6 +115,7 @@ project "App_BulletExampleBrowser" "../SharedMemory/PhysicsServerCommandProcessor.cpp", "../SharedMemory/PhysicsServerCommandProcessor.h", "../SharedMemory/b3PluginManager.cpp", + "../SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp", "../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp", "../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", diff --git a/examples/SharedMemory/CMakeLists.txt b/examples/SharedMemory/CMakeLists.txt index 2d126396d..7bc68ab2b 100644 --- a/examples/SharedMemory/CMakeLists.txt +++ b/examples/SharedMemory/CMakeLists.txt @@ -1,5 +1,7 @@ SET(SharedMemory_SRCS + plugins/collisionFilterPlugin/collisionFilterPlugin.cpp + plugins/collisionFilterPlugin/collisionFilterPlugin.h plugins/pdControlPlugin/pdControlPlugin.cpp plugins/pdControlPlugin/pdControlPlugin.h b3RobotSimulatorClientAPI_NoDirect.cpp diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index e95d597fa..adaed8fbc 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -46,6 +46,10 @@ #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h" #include "LinearMath/TaskScheduler/btThreadSupportInterface.h" +#ifndef SKIP_COLLISION_FILTER_PLUGIN +#include "plugins/collisionFilterPlugin/collisionFilterPlugin.h" +#endif + #ifndef SKIP_STATIC_PD_CONTROL_PLUGIN #include "plugins/pdControlPlugin/pdControlPlugin.h" #endif //SKIP_STATIC_PD_CONTROL_PLUGIN @@ -1625,6 +1629,8 @@ struct PhysicsServerCommandProcessorInternalData btScalar m_oldPickingDist; bool m_prevCanSleep; int m_pdControlPlugin; + int m_collisionFilterPlugin; + #ifdef B3_ENABLE_TINY_AUDIO b3SoundEngine m_soundEngine; #endif @@ -1662,6 +1668,7 @@ struct PhysicsServerCommandProcessorInternalData m_pickedConstraint(0), m_pickingMultiBodyPoint2Point(0), m_pdControlPlugin(-1), + m_collisionFilterPlugin(-1), m_threadPool(0) { @@ -1677,6 +1684,11 @@ struct PhysicsServerCommandProcessorInternalData } #endif //SKIP_STATIC_PD_CONTROL_PLUGIN +#ifndef SKIP_COLLISION_FILTER_PLUGIN + { + m_collisionFilterPlugin = m_pluginManager.registerStaticLinkedPlugin("collisionFilterPlugin", initPlugin_collisionFilterPlugin, exitPlugin_collisionFilterPlugin, executePluginCommand_collisionFilterPlugin, 0,0,0); + } +#endif #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN int renderPluginId = m_pluginManager.registerStaticLinkedPlugin("tinyRendererPlugin", initPlugin_tinyRendererPlugin, exitPlugin_tinyRendererPlugin, executePluginCommand_tinyRendererPlugin,0,0,getRenderInterface_tinyRendererPlugin); diff --git a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp index d61937271..6e962470f 100644 --- a/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp +++ b/examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp @@ -8,22 +8,22 @@ #include "../b3PluginContext.h" #include -struct MyClass +struct CollisionFilterMyClass { int m_testData; - MyClass() + CollisionFilterMyClass() :m_testData(42) { } - virtual ~MyClass() + virtual ~CollisionFilterMyClass() { } }; -B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context) +B3_SHARED_API int initPlugin_collisionFilterPlugin(struct b3PluginContext* context) { - MyClass* obj = new MyClass(); + CollisionFilterMyClass* obj = new CollisionFilterMyClass(); context->m_userPointer = obj; printf("hi!\n"); @@ -31,21 +31,21 @@ B3_SHARED_API int initPlugin_testPlugin(struct b3PluginContext* context) } -B3_SHARED_API int preTickPluginCallback_testPlugin(struct b3PluginContext* context) +B3_SHARED_API int preTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context) { //apply pd control here, apply forces using the PD gains return 0; } -B3_SHARED_API int postTickPluginCallback_testPlugin(struct b3PluginContext* context) +B3_SHARED_API int postTickPluginCallback_collisionFilterPlugin(struct b3PluginContext* context) { - MyClass* obj = (MyClass* )context->m_userPointer; + CollisionFilterMyClass* obj = (CollisionFilterMyClass* )context->m_userPointer; obj->m_testData++; return 0; } -B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) +B3_SHARED_API int executePluginCommand_collisionFilterPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments) { //set the PD gains printf("text argument:%s\n",arguments->m_text); @@ -69,7 +69,7 @@ B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* contex } printf("]\n"); - MyClass* obj = (MyClass*) context->m_userPointer; + CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer; b3SharedMemoryStatusHandle statusHandle; int statusType = -1; @@ -88,9 +88,9 @@ B3_SHARED_API int executePluginCommand_testPlugin(struct b3PluginContext* contex } -B3_SHARED_API void exitPlugin_testPlugin(struct b3PluginContext* context) +B3_SHARED_API void exitPlugin_collisionFilterPlugin(struct b3PluginContext* context) { - MyClass* obj = (MyClass*) context->m_userPointer; + CollisionFilterMyClass* obj = (CollisionFilterMyClass*) context->m_userPointer; delete obj; context->m_userPointer = 0; diff --git a/examples/SharedMemory/premake4.lua b/examples/SharedMemory/premake4.lua index be66e7e42..6330786cf 100644 --- a/examples/SharedMemory/premake4.lua +++ b/examples/SharedMemory/premake4.lua @@ -60,6 +60,7 @@ myfiles = "PhysicsServerCommandProcessor.h", "b3PluginManager.cpp", "b3PluginManager.h", + "plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "plugins/pdControlPlugin/pdControlPlugin.cpp", "plugins/pdControlPlugin/pdControlPlugin.h", "../OpenGLWindow/SimpleCamera.cpp", @@ -472,4 +473,3 @@ include "plugins/tinyRendererPlugin" include "plugins/pdControlPlugin" include "plugins/collisionFilterPlugin" - diff --git a/examples/SharedMemory/tcp/premake4.lua b/examples/SharedMemory/tcp/premake4.lua index a4879953d..ac96b300e 100644 --- a/examples/SharedMemory/tcp/premake4.lua +++ b/examples/SharedMemory/tcp/premake4.lua @@ -93,6 +93,7 @@ myfiles = "../PhysicsDirect.cpp", "../PhysicsClientC_API.cpp", "../PhysicsClient.cpp", + "../plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../plugins/pdControlPlugin/pdControlPlugin.cpp", "../plugins/pdControlPlugin/pdControlPlugin.h", "../b3RobotSimulatorClientAPI_NoDirect.cpp", diff --git a/examples/SharedMemory/udp/premake4.lua b/examples/SharedMemory/udp/premake4.lua index f9b7f8c4b..f0d046c0d 100644 --- a/examples/SharedMemory/udp/premake4.lua +++ b/examples/SharedMemory/udp/premake4.lua @@ -77,6 +77,7 @@ language "C++" myfiles = { + "../plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../plugins/pdControlPlugin/pdControlPlugin.cpp", "../plugins/pdControlPlugin/pdControlPlugin.h", "../b3RobotSimulatorClientAPI_NoDirect.cpp", diff --git a/examples/TwoJoint/CMakeLists.txt b/examples/TwoJoint/CMakeLists.txt index 966b5e2a6..a596ab404 100644 --- a/examples/TwoJoint/CMakeLists.txt +++ b/examples/TwoJoint/CMakeLists.txt @@ -10,6 +10,7 @@ INCLUDE_DIRECTORIES( SET(RobotSimulator_SRCS TwoJointMain.cpp + ../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp diff --git a/examples/Utils/ChromeTraceUtil.cpp b/examples/Utils/ChromeTraceUtil.cpp index fefa9171c..9027b4202 100644 --- a/examples/Utils/ChromeTraceUtil.cpp +++ b/examples/Utils/ChromeTraceUtil.cpp @@ -4,6 +4,7 @@ #include "LinearMath/btQuickprof.h" #include "LinearMath/btAlignedObjectArray.h" #include "Bullet3Common/b3Logging.h" +#include struct btTiming { @@ -155,13 +156,13 @@ struct btTimings int m_activeBuffer; btAlignedObjectArray m_timings[1]; }; -#ifndef BT_NO_PROFILE +//#ifndef BT_NO_PROFILE btTimings gTimings[BT_QUICKPROF_MAX_THREAD_COUNT]; #define MAX_NESTING 1024 int gStackDepths[BT_QUICKPROF_MAX_THREAD_COUNT] = { 0 }; const char* gFuncNames[BT_QUICKPROF_MAX_THREAD_COUNT][MAX_NESTING]; unsigned long long int gStartTimes[BT_QUICKPROF_MAX_THREAD_COUNT][MAX_NESTING]; -#endif +//#endif btClock clk; @@ -276,4 +277,4 @@ void b3ChromeUtilsStopTimingsAndWriteJsonFile(const char* fileNamePrefix) void b3ChromeUtilsEnableProfiling() { gProfileDisabled = false; -} \ No newline at end of file +} diff --git a/examples/pybullet/CMakeLists.txt b/examples/pybullet/CMakeLists.txt index d01ad1655..f3cacc5b3 100644 --- a/examples/pybullet/CMakeLists.txt +++ b/examples/pybullet/CMakeLists.txt @@ -15,6 +15,7 @@ ENDIF() SET(pybullet_SRCS pybullet.c + ../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp diff --git a/setup.py b/setup.py index cfb5f5b8e..cfea0b36b 100644 --- a/setup.py +++ b/setup.py @@ -50,6 +50,7 @@ sources = ["examples/pybullet/pybullet.c"]\ +["examples/TinyRenderer/our_gl.cpp"]\ +["examples/TinyRenderer/TinyRenderer.cpp"]\ +["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\ ++["examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp"]\ +["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\ +["examples/SharedMemory/IKTrajectoryHelper.cpp"]\ +["examples/SharedMemory/InProcessMemory.cpp"]\ diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index 7b38d71b9..e3afc5a80 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -65,16 +65,19 @@ btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc(); void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc); void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc); -#ifndef BT_NO_PROFILE // FIX redefinition +#ifndef BT_ENABLE_PROFILE //To disable built-in profiling, please comment out next line -//#define BT_NO_PROFILE 1 +#ifndef BT_NO_PROFILE +#define BT_NO_PROFILE 1 #endif //BT_NO_PROFILE +#endif //BT_ENABLE_PROFILE + +const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64; #ifndef BT_NO_PROFILE //btQuickprofGetCurrentThreadIndex will return -1 if thread index cannot be determined, //otherwise returns thread index in range [0..maxThreads] unsigned int btQuickprofGetCurrentThreadIndex2(); -const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64; #include //@todo remove this, backwards compatibility diff --git a/test/SharedMemory/CMakeLists.txt b/test/SharedMemory/CMakeLists.txt index 78746108b..5835d1991 100644 --- a/test/SharedMemory/CMakeLists.txt +++ b/test/SharedMemory/CMakeLists.txt @@ -29,6 +29,7 @@ ENDIF() ADD_EXECUTABLE(Test_PhysicsClientServer gtestwrap.cpp ../../examples/SharedMemory/PhysicsClient.cpp + ../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp ../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h ../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp diff --git a/test/SharedMemory/premake4.lua b/test/SharedMemory/premake4.lua index f51f77e04..01676802e 100644 --- a/test/SharedMemory/premake4.lua +++ b/test/SharedMemory/premake4.lua @@ -172,6 +172,7 @@ project ("Test_PhysicsServerLoopBack") "test.c", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", @@ -264,6 +265,7 @@ end "test.c", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", @@ -372,6 +374,7 @@ project ("Test_PhysicsServerInProcessExampleBrowser") "test.c", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp", "../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h", + "../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", "../../examples/SharedMemory/IKTrajectoryHelper.cpp", From 5dd7a623971d8c9d0e22825f2f8a14c24c8e7f94 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 30 Jul 2018 12:07:32 +0200 Subject: [PATCH 2/8] Disable CProfileManager by default. Use btSetCustomEnterProfileZoneFunc(CProfileManager::Start_Profile) and btSetCustomLeaveProfileZoneFunc(CProfileManager::Stop_Profile) to get old behavior. --- src/LinearMath/btQuickprof.cpp | 2 -- src/LinearMath/btQuickprof.h | 6 ++---- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/LinearMath/btQuickprof.cpp b/src/LinearMath/btQuickprof.cpp index d2b970152..1572b9626 100644 --- a/src/LinearMath/btQuickprof.cpp +++ b/src/LinearMath/btQuickprof.cpp @@ -729,11 +729,9 @@ unsigned int btQuickprofGetCurrentThreadIndex2() { void btEnterProfileZoneDefault(const char* name) { - CProfileManager::Start_Profile( name ); } void btLeaveProfileZoneDefault() { - CProfileManager::Stop_Profile(); } diff --git a/src/LinearMath/btQuickprof.h b/src/LinearMath/btQuickprof.h index e3afc5a80..98a267577 100644 --- a/src/LinearMath/btQuickprof.h +++ b/src/LinearMath/btQuickprof.h @@ -65,12 +65,10 @@ btLeaveProfileZoneFunc* btGetCurrentLeaveProfileZoneFunc(); void btSetCustomEnterProfileZoneFunc(btEnterProfileZoneFunc* enterFunc); void btSetCustomLeaveProfileZoneFunc(btLeaveProfileZoneFunc* leaveFunc); -#ifndef BT_ENABLE_PROFILE +#ifndef BT_NO_PROFILE // FIX redefinition //To disable built-in profiling, please comment out next line -#ifndef BT_NO_PROFILE -#define BT_NO_PROFILE 1 +//#define BT_NO_PROFILE 1 #endif //BT_NO_PROFILE -#endif //BT_ENABLE_PROFILE const unsigned int BT_QUICKPROF_MAX_THREAD_COUNT = 64; From e75178181d0011041124dc3a8e59eeecb35e8eff Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 30 Jul 2018 12:21:02 +0200 Subject: [PATCH 3/8] safe normalize on degenerate triangles --- examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp index a4f4b9b3a..378201610 100644 --- a/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp +++ b/examples/ExampleBrowser/CollisionShape2TriangleMesh.cpp @@ -158,7 +158,7 @@ void CollisionShape2TriangleMesh(btCollisionShape* collisionShape, const btTrans btVector3 pos1 =parentTransform*hull->getVertexPointer()[index1]; btVector3 pos2 =parentTransform*hull->getVertexPointer()[index2]; triNormal = (pos1-pos0).cross(pos2-pos0); - triNormal.normalize(); + triNormal.safeNormalize(); for (int v=0;v<3;v++) { From 2000ba905810d93277823e19ffd5f94dc4133bdf Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 30 Jul 2018 17:30:19 +0200 Subject: [PATCH 4/8] handle singularity (gimbal lock) in quaternion -> euler conversion, from https://github.com/ros/urdfdom_headers/blob/e7e0972a4617b8a5df7a274ea3ba3b92e3895a35/urdf_model/include/urdf_model/pose.h#L103 --- src/LinearMath/btQuaternion.h | 26 ++++++++++++++++++++++---- 1 file changed, 22 insertions(+), 4 deletions(-) diff --git a/src/LinearMath/btQuaternion.h b/src/LinearMath/btQuaternion.h index c29b0be65..a98fec7bc 100644 --- a/src/LinearMath/btQuaternion.h +++ b/src/LinearMath/btQuaternion.h @@ -173,10 +173,28 @@ public: sqy = m_floats[1] * m_floats[1]; sqz = m_floats[2] * m_floats[2]; squ = m_floats[3] * m_floats[3]; - rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); - sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); - pitchY = sarg <= btScalar(-1.0) ? btScalar(-0.5) * SIMD_PI: (sarg >= btScalar(1.0) ? btScalar(0.5) * SIMD_PI : btAsin(sarg)); - yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + sarg = btScalar(-2.) * (m_floats[0] * m_floats[2] - m_floats[3] * m_floats[1]); + + // If the pitch angle is PI/2 or -PI/2, we can only compute + // the sum roll + yaw. However, any combination that gives + // the right sum will produce the correct orientation, so we + // set rollX = 0 and compute yawZ. + if (sarg <= -btScalar(0.99999)) + { + pitchY = btScalar(-0.5)*SIMD_PI; + rollX = 0; + yawZ = btScalar(2) * btAtan2(m_floats[0],-m_floats[1]); + } else if (sarg >= btScalar(0.99999)) + { + pitchY = btScalar(0.5)*SIMD_PI; + rollX = 0; + yawZ = btScalar(2) * btAtan2(-m_floats[0], m_floats[1]); + } else + { + pitchY = btAsin(sarg); + rollX = btAtan2(2 * (m_floats[1] * m_floats[2] + m_floats[3] * m_floats[0]), squ - sqx - sqy + sqz); + yawZ = btAtan2(2 * (m_floats[0] * m_floats[1] + m_floats[3] * m_floats[2]), squ + sqx - sqy - sqz); + } } /**@brief Add two quaternions From 08b5e6d7669611704ef6862f02fec689e5d2a8be Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Mon, 30 Jul 2018 17:53:37 +0200 Subject: [PATCH 5/8] add collisionFilterPlugin to pybullet plugin, also apply quat->euler fix for singular cases --- examples/pybullet/premake4.lua | 1 + examples/pybullet/pybullet.c | 28 ++++++++++++++++++++++------ 2 files changed, 23 insertions(+), 6 deletions(-) diff --git a/examples/pybullet/premake4.lua b/examples/pybullet/premake4.lua index 04380b9b0..60d1a1cf9 100644 --- a/examples/pybullet/premake4.lua +++ b/examples/pybullet/premake4.lua @@ -155,6 +155,7 @@ if not _OPTIONS["no-enet"] then "../../examples/MultiThreading/b3PosixThreadSupport.cpp", "../../examples/MultiThreading/b3Win32ThreadSupport.cpp", "../../examples/MultiThreading/b3ThreadSupportInterface.cpp", + "../../examples/SharedMemory/plugins/collisionFilterPlugin/collisionFilterPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp", "../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h", } diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 836f06312..cae7bb816 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -4,6 +4,7 @@ #ifdef BT_ENABLE_ENET #include "../SharedMemory/PhysicsClientUDP_C_API.h" #endif //BT_ENABLE_ENET +#define PYBULLET_PI (3.1415926535897932384626433832795029) #ifdef BT_ENABLE_DART #include "../SharedMemory/dart/DARTPhysicsC_API.h" @@ -8239,13 +8240,28 @@ static PyObject* pybullet_getEulerFromQuaternion(PyObject* self, sqy = quat[1] * quat[1]; sqz = quat[2] * quat[2]; squ = quat[3] * quat[3]; - rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), - squ - sqx - sqy + sqz); sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]); - rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538 - : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg)); - rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), - squ + sqx - sqy - sqz); + + // If the pitch angle is PI/2 or -PI/2, we can only compute + // the sum roll + yaw. However, any combination that gives + // the right sum will produce the correct orientation, so we + // set rollX = 0 and compute yawZ. + if (sarg <= -0.99999) + { + rpy[0] = 0; + rpy[1] = -0.5*PYBULLET_PI; + rpy[2] = 2 * atan2(quat[0],-quat[1]); + } else if (sarg >= 0.99999) + { + rpy[0] = 0; + rpy[1] = 0.5*PYBULLET_PI; + rpy[2] = 2 * atan2(-quat[0], quat[1]); + } else + { + rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]), squ - sqx - sqy + sqz); + rpy[1] = asin(sarg); + rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]), squ + sqx - sqy - sqz); + } { PyObject* pylist; int i; From 97baccd1a5b4962b17a75c508de2080382eaa3e1 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 3 Aug 2018 17:38:01 -0700 Subject: [PATCH 6/8] revert regression in btMultiBody.cpp --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 8023f63c0..6bb0a2f8d 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -970,6 +970,9 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar - spatCoriolisAcc[i].dot(hDof) ; + } + for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) + } btScalar *D_row = &D[dof * m_links[i].m_dofCount]; for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) { From 02b0e34bdae22b94734f4c72520030bc67110f91 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 3 Aug 2018 17:53:07 -0700 Subject: [PATCH 7/8] v_ptr --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index 6bb0a2f8d..a4014adde 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -781,7 +781,7 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // hhat is NOT stored for the base (but ahat is) btSpatialForceVector * h = (btSpatialForceVector *)(m_dofCount > 0 ? &m_vectorBuf[0] : 0); btSpatialMotionVector * spatAcc = (btSpatialMotionVector *)v_ptr; - // v_ptr += num_links * 2 + 2; // Disabled since v_ptr is not used in the rest of the code + v_ptr += num_links * 2 + 2; // // Y_i, invD_i btScalar * invD = m_dofCount > 0 ? &m_realBuf[6 + m_dofCount] : 0; From 684b69f4ddb48748b0eb2ab5ea9fd49418bd0c6a Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Fri, 3 Aug 2018 17:57:09 -0700 Subject: [PATCH 8/8] fix applied too fast --- src/BulletDynamics/Featherstone/btMultiBody.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/BulletDynamics/Featherstone/btMultiBody.cpp b/src/BulletDynamics/Featherstone/btMultiBody.cpp index a4014adde..34fe4b8de 100644 --- a/src/BulletDynamics/Featherstone/btMultiBody.cpp +++ b/src/BulletDynamics/Featherstone/btMultiBody.cpp @@ -967,12 +967,11 @@ void btMultiBody::computeAccelerationsArticulatedBodyAlgorithmMultiDof(btScalar // Y[m_links[i].m_dofOffset + dof] = m_links[i].m_jointTorque[dof] - m_links[i].m_axes[dof].dot(zeroAccSpatFrc[i+1]) - - spatCoriolisAcc[i].dot(hDof) - ; + - spatCoriolisAcc[i].dot(hDof); } for(int dof = 0; dof < m_links[i].m_dofCount; ++dof) - } + { btScalar *D_row = &D[dof * m_links[i].m_dofCount]; for(int dof2 = 0; dof2 < m_links[i].m_dofCount; ++dof2) {