diff --git a/examples/ExampleBrowser/CMakeLists.txt b/examples/ExampleBrowser/CMakeLists.txt index 331bb1900..2416b51a2 100644 --- a/examples/ExampleBrowser/CMakeLists.txt +++ b/examples/ExampleBrowser/CMakeLists.txt @@ -328,6 +328,7 @@ SET(BulletExampleBrowser_SRCS ../MultiBody/InvertedPendulumPDControl.cpp ../MultiBody/InvertedPendulumPDControl.h ../MultiBody/MultiBodyConstraintFeedback.cpp + ../MultiBody/SerialChains.cpp ../MultiBody/MultiDofDemo.cpp ../MultiBody/MultiDofDemo.h ../RigidBody/RigidBodySoftContact.cpp diff --git a/examples/ExampleBrowser/ExampleEntries.cpp b/examples/ExampleBrowser/ExampleEntries.cpp index f7f2f9b63..f3973c565 100644 --- a/examples/ExampleBrowser/ExampleEntries.cpp +++ b/examples/ExampleBrowser/ExampleEntries.cpp @@ -29,6 +29,7 @@ #include "../MultiBody/MultiBodyConstraintFeedback.h" #include "../MultiBody/MultiDofDemo.h" #include "../MultiBody/InvertedPendulumPDControl.h" +#include "../MultiBody/SerialChains.h" #include "../RigidBody/RigidBodySoftContact.h" #include "../VoronoiFracture/VoronoiFractureDemo.h" #include "../SoftDemo/SoftDemo.h" @@ -137,6 +138,7 @@ static ExampleEntry gDefaultExamples[]= ExampleEntry(1,"Constraint Feedback", "The example shows how to receive joint reaction forces in a btMultiBody. Also the applied impulse is available for a btMultiBodyJointMotor", MultiBodyConstraintFeedbackCreateFunc), ExampleEntry(1,"Inverted Pendulum PD","Keep an inverted pendulum up using open loop PD control", InvertedPendulumPDControlCreateFunc), ExampleEntry(1,"MultiBody Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.",MultiBodySoftContactCreateFunc,0), + ExampleEntry(1,"Serial Chains", "Show colliding two serial chains using different constraint solvers.", SerialChainsCreateFunc,0), ExampleEntry(0,"Physics Client-Server"), ExampleEntry(1,"Physics Server", "Create a physics server that communicates with a physics client over shared memory. You can connect to the server using pybullet, a PhysicsClient or a UDP/TCP Bridge.", diff --git a/examples/ExampleBrowser/premake4.lua b/examples/ExampleBrowser/premake4.lua index 1ed5e0965..0e3c7975e 100644 --- a/examples/ExampleBrowser/premake4.lua +++ b/examples/ExampleBrowser/premake4.lua @@ -162,6 +162,7 @@ project "App_BulletExampleBrowser" "../Vehicles/*", "../Raycast/*", "../MultiBody/MultiDofDemo.cpp", + "../MultiBody/SerialChains.cpp", "../MultiBody/TestJointTorqueSetup.cpp", "../MultiBody/Pendulum.cpp", "../MultiBody/MultiBodySoftContact.cpp", diff --git a/examples/MultiBody/MultiDofDemo.cpp b/examples/MultiBody/MultiDofDemo.cpp index 86d893f73..d0530f9fb 100644 --- a/examples/MultiBody/MultiDofDemo.cpp +++ b/examples/MultiBody/MultiDofDemo.cpp @@ -2,8 +2,13 @@ #include "../OpenGLWindow/SimpleOpenGL3App.h" #include "btBulletDynamicsCommon.h" +#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h" +#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h" +#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h" + #include "BulletDynamics/Featherstone/btMultiBody.h" #include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h" +#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h" #include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h" #include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h" #include "BulletDynamics/Featherstone/btMultiBodyLink.h" @@ -40,7 +45,6 @@ public: m_guiHelper->resetCamera(dist,yaw,pitch,targetPos[0],targetPos[1],targetPos[2]); } - btMultiBody* createFeatherstoneMultiBody_testMultiDof(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents, bool spherical = false, bool floating = false); void addColliders_testMultiDof(btMultiBody *pMultiBody, btMultiBodyDynamicsWorld *pWorld, const btVector3 &baseHalfExtents, const btVector3 &linkHalfExtents); void addBoxes_testMultiDof(); @@ -52,6 +56,7 @@ static bool g_floatingBase = false; static bool g_firstInit = true; static float scaling = 0.4f; static float friction = 1.; +static int g_constraintSolverType = 0; #define ARRAY_SIZE_X 5 #define ARRAY_SIZE_Y 5 #define ARRAY_SIZE_Z 5 @@ -104,8 +109,37 @@ void MultiDofDemo::initPhysics() m_broadphase = new btDbvtBroadphase(); - //Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support - btMultiBodyConstraintSolver* sol = new btMultiBodyConstraintSolver; + if (g_constraintSolverType == 4) + { + g_constraintSolverType = 0; + g_floatingBase = !g_floatingBase; + } + + btMultiBodyConstraintSolver* sol; + btMLCPSolverInterface* mlcp; + switch (g_constraintSolverType++) + { + case 0: + sol = new btMultiBodyConstraintSolver; + b3Printf("Constraint Solver: Sequential Impulse"); + break; + case 1: + mlcp = new btSolveProjectedGaussSeidel(); + sol = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + PGS"); + break; + case 2: + mlcp = new btDantzigSolver(); + sol = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + Dantzig"); + break; + default: + mlcp = new btLemkeSolver(); + sol = new btMultiBodyMLCPConstraintSolver(mlcp); + b3Printf("Constraint Solver: MLCP + Lemke"); + break; + } + m_solver = sol; //use btMultiBodyDynamicsWorld for Featherstone btMultiBody support @@ -114,6 +148,7 @@ void MultiDofDemo::initPhysics() // m_dynamicsWorld->setDebugDrawer(&gDebugDraw); m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld); m_dynamicsWorld->setGravity(btVector3(0,-10,0)); + m_dynamicsWorld->getSolverInfo().m_globalCfm = 1e-3; ///create a few basic rigid bodies btVector3 groundHalfExtents(50,50,50); @@ -135,7 +170,7 @@ void MultiDofDemo::initPhysics() int numLinks = 5; bool spherical = true; //set it ot false -to use 1DoF hinges instead of 3DoF sphericals bool multibodyOnly = false; - bool canSleep = true; + bool canSleep = false; bool selfCollide = true; bool multibodyConstraint = false; btVector3 linkHalfExtents(0.05, 0.37, 0.1); @@ -144,7 +179,6 @@ void MultiDofDemo::initPhysics() btMultiBody* mbC = createFeatherstoneMultiBody_testMultiDof(world, numLinks, btVector3(-0.4f, 3.f, 0.f), linkHalfExtents, baseHalfExtents, spherical, g_floatingBase); //mbC->forceMultiDof(); //if !spherical, you can comment this line to check the 1DoF algorithm - g_floatingBase = ! g_floatingBase; mbC->setCanSleep(canSleep); mbC->setHasSelfCollision(selfCollide); mbC->setUseGyroTerm(gyro); diff --git a/examples/SharedMemory/b3PluginManager.cpp b/examples/SharedMemory/b3PluginManager.cpp index b4ab56487..52f7b05b8 100644 --- a/examples/SharedMemory/b3PluginManager.cpp +++ b/examples/SharedMemory/b3PluginManager.cpp @@ -327,7 +327,7 @@ void b3PluginManager::reportNotifications() plugin->m_processNotificationsFunc(&context); } } - notifications.clear(); + notifications.resize(0); } int b3PluginManager::executePluginCommand(int pluginUniqueId, const b3PluginArguments* arguments) diff --git a/setup.py b/setup.py index cfea0b36b..cc4d88e67 100644 --- a/setup.py +++ b/setup.py @@ -453,7 +453,7 @@ print("-----") setup( name = 'pybullet', - version='2.0.9', + version='2.1.1', description='Official Python Interface for the Bullet Physics SDK specialized for Robotics Simulation and Reinforcement Learning', long_description='pybullet is an easy to use Python module for physics simulation, robotics and deep reinforcement learning based on the Bullet Physics SDK. With pybullet you can load articulated bodies from URDF, SDF and other file formats. pybullet provides forward dynamics simulation, inverse dynamics computation, forward and inverse kinematics and collision detection and ray intersection queries. Aside from physics simulation, pybullet supports to rendering, with a CPU renderer and OpenGL visualization and support for virtual reality headsets.', url='https://github.com/bulletphysics/bullet3', diff --git a/src/BulletDynamics/CMakeLists.txt b/src/BulletDynamics/CMakeLists.txt index 2eb03c39a..d416d7d5f 100644 --- a/src/BulletDynamics/CMakeLists.txt +++ b/src/BulletDynamics/CMakeLists.txt @@ -31,15 +31,16 @@ SET(BulletDynamics_SRCS Vehicle/btRaycastVehicle.cpp Vehicle/btWheelInfo.cpp Featherstone/btMultiBody.cpp + Featherstone/btMultiBodyConstraint.cpp Featherstone/btMultiBodyConstraintSolver.cpp Featherstone/btMultiBodyDynamicsWorld.cpp - Featherstone/btMultiBodyJointLimitConstraint.cpp - Featherstone/btMultiBodyConstraint.cpp - Featherstone/btMultiBodyPoint2Point.cpp Featherstone/btMultiBodyFixedConstraint.cpp - Featherstone/btMultiBodySliderConstraint.cpp - Featherstone/btMultiBodyJointMotor.cpp Featherstone/btMultiBodyGearConstraint.cpp + Featherstone/btMultiBodyJointLimitConstraint.cpp + Featherstone/btMultiBodyJointMotor.cpp + Featherstone/btMultiBodyMLCPConstraintSolver.cpp + Featherstone/btMultiBodyPoint2Point.cpp + Featherstone/btMultiBodySliderConstraint.cpp MLCPSolvers/btDantzigLCP.cpp MLCPSolvers/btMLCPSolver.cpp MLCPSolvers/btLemkeAlgorithm.cpp @@ -90,19 +91,19 @@ SET(Vehicle_HDRS SET(Featherstone_HDRS Featherstone/btMultiBody.h + Featherstone/btMultiBodyConstraint.h Featherstone/btMultiBodyConstraintSolver.h Featherstone/btMultiBodyDynamicsWorld.h + Featherstone/btMultiBodyFixedConstraint.h + Featherstone/btMultiBodyGearConstraint.h + Featherstone/btMultiBodyJointLimitConstraint.h + Featherstone/btMultiBodyJointMotor.h Featherstone/btMultiBodyLink.h Featherstone/btMultiBodyLinkCollider.h - Featherstone/btMultiBodySolverConstraint.h - Featherstone/btMultiBodyConstraint.h - Featherstone/btMultiBodyJointLimitConstraint.h - Featherstone/btMultiBodyConstraint.h + Featherstone/btMultiBodyMLCPConstraintSolver.h Featherstone/btMultiBodyPoint2Point.h - Featherstone/btMultiBodyFixedConstraint.h Featherstone/btMultiBodySliderConstraint.h - Featherstone/btMultiBodyJointMotor.h - Featherstone/btMultiBodyGearConstraint.h + Featherstone/btMultiBodySolverConstraint.h ) SET(MLCPSolvers_HDRS