Merge remote-tracking branch 'bp/master'

examples/BlockSolver/BlockSolverExample.cpp

@@ -0,0 +1,219 @@
+#include "BlockSolverExample.h"
+#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
+#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
+#include "btBlockSolver.h"
+//for URDF import support
+#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
+#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
+#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
+#include "../CommonInterfaces/CommonMultiBodyBase.h"
+
+class BlockSolverExample : public CommonMultiBodyBase
+{
+	int m_option;
+public:
+	BlockSolverExample(GUIHelperInterface* helper, int option);
+	virtual ~BlockSolverExample();
+
+	virtual void initPhysics();
+
+	virtual void stepSimulation(float deltaTime);
+
+	virtual void resetCamera()
+	{
+		float dist = 3;
+		float pitch = -35;
+		float yaw = 50;
+		float targetPos[3] = {0, 0, .1};
+		m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+
+	void createMultiBodyStack();
+	btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape);
+	btMultiBody* loadRobot(std::string filepath);
+};
+
+
+BlockSolverExample::BlockSolverExample(GUIHelperInterface* helper, int option)
+	: CommonMultiBodyBase(helper),
+	m_option(option)
+{
+	m_guiHelper->setUpAxis(2);
+}
+
+BlockSolverExample::~BlockSolverExample()
+{
+	// Do nothing
+}
+
+void BlockSolverExample::stepSimulation(float deltaTime)
+{
+	//use a smaller internal timestep, there are stability issues
+	btScalar internalTimeStep = 1./240.f;
+	m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
+}
+
+void BlockSolverExample::initPhysics()
+{
+	
+	///collision configuration contains default setup for memory, collision setup
+	m_collisionConfiguration = new btDefaultCollisionConfiguration();
+
+	///use the default collision dispatcher. For parallel processing you can use a diffent dispatcher (see Extras/BulletMultiThreaded)
+	m_dispatcher = new btCollisionDispatcher(m_collisionConfiguration);
+
+	m_broadphase = new btDbvtBroadphase();
+
+
+	btMLCPSolverInterface* mlcp;
+	if (m_option&BLOCK_SOLVER_SI)
+	{
+		btAssert(!m_solver);
+		m_solver = new btMultiBodyConstraintSolver;
+		b3Printf("Constraint Solver: Sequential Impulse");
+	}
+	if (m_option&BLOCK_SOLVER_MLCP_PGS)
+	{
+		btAssert(!m_solver);
+		mlcp = new btSolveProjectedGaussSeidel();
+		m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+		b3Printf("Constraint Solver: MLCP + PGS");
+	}
+	if (m_option&BLOCK_SOLVER_MLCP_DANTZIG)
+	{
+		btAssert(!m_solver);
+		mlcp = new btDantzigSolver();
+		m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
+		b3Printf("Constraint Solver: MLCP + Dantzig");
+	}
+	if (m_option&BLOCK_SOLVER_BLOCK)
+	{
+		m_solver = new btBlockSolver();
+	}
+
+	btAssert(m_solver);
+
+	
+
+	btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
+	m_dynamicsWorld = world;
+	m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
+	m_dynamicsWorld->setGravity(btVector3(0, 0, -10));
+	m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
+	m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-6);  //todo: what value is good?
+
+	if (m_option&BLOCK_SOLVER_SCENE_MB_STACK)
+	{
+		createMultiBodyStack();
+	}
+	
+
+	m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
+}
+
+
+void BlockSolverExample::createMultiBodyStack()
+{
+	///create a few basic rigid bodies
+	bool loadPlaneFromURDF = false;
+	if (loadPlaneFromURDF)
+	{
+		btMultiBody* mb = loadRobot("plane.urdf");
+		printf("!\n");
+	}
+	else
+	{
+		btBoxShape* groundShape = createBoxShape(btVector3(btScalar(50.), btScalar(50.), btScalar(50.)));
+		m_collisionShapes.push_back(groundShape);
+		btScalar mass = 0;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, -50));
+		btMultiBody* body = createMultiBody(mass, tr, groundShape);
+	}
+
+	for (int i=0;i<10;i++)
+	{
+		btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
+		m_collisionShapes.push_back(boxShape);
+		btScalar mass = 1;
+		if (i == 9)
+			mass = 100;
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 0.1+i*0.2));
+		btMultiBody* body = createMultiBody(mass, tr, boxShape);
+	}
+	if(0)
+	{
+		btMultiBody* mb = loadRobot("cube_small.urdf");
+		btTransform tr;
+		tr.setIdentity();
+		tr.setOrigin(btVector3(0, 0, 1.));
+		mb->setBaseWorldTransform(tr);
+	}
+}
+
+btMultiBody* BlockSolverExample::createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape)
+{
+	btVector3 inertia;
+	collisionShape->calculateLocalInertia(mass, inertia);
+
+	bool canSleep = false;
+	bool isDynamic = mass > 0;
+	btMultiBody* mb = new btMultiBody(0, mass, inertia, !isDynamic, canSleep);
+	btMultiBodyLinkCollider* collider = new btMultiBodyLinkCollider(mb, -1);
+	collider->setWorldTransform(trans);
+	mb->setBaseWorldTransform(trans);
+
+	collider->setCollisionShape(collisionShape);
+
+	int collisionFilterGroup = isDynamic ? int(btBroadphaseProxy::DefaultFilter) : int(btBroadphaseProxy::StaticFilter);
+	int collisionFilterMask = isDynamic ? int(btBroadphaseProxy::AllFilter) : int(btBroadphaseProxy::AllFilter ^ btBroadphaseProxy::StaticFilter);
+
+
+	this->m_dynamicsWorld->addCollisionObject(collider, collisionFilterGroup, collisionFilterMask);
+	mb->setBaseCollider(collider);
+
+	mb->finalizeMultiDof();
+
+
+	this->m_dynamicsWorld->addMultiBody(mb);
+	m_dynamicsWorld->forwardKinematics();
+	return mb;
+}
+
+
+
+btMultiBody* BlockSolverExample::loadRobot(std::string filepath)
+{
+	btMultiBody* m_multiBody = 0;
+	BulletURDFImporter u2b(m_guiHelper, 0, 0, 1, 0);
+	bool loadOk = u2b.loadURDF(filepath.c_str());// lwr / kuka.urdf");
+	if (loadOk)
+	{
+		int rootLinkIndex = u2b.getRootLinkIndex();
+		b3Printf("urdf root link index = %d\n", rootLinkIndex);
+		MyMultiBodyCreator creation(m_guiHelper);
+		btTransform identityTrans;
+		identityTrans.setIdentity();
+		ConvertURDF2Bullet(u2b, creation, identityTrans, m_dynamicsWorld, true, u2b.getPathPrefix());
+		for (int i = 0; i < u2b.getNumAllocatedCollisionShapes(); i++)
+		{
+			m_collisionShapes.push_back(u2b.getAllocatedCollisionShape(i));
+		}
+		m_multiBody = creation.getBulletMultiBody();
+		if (m_multiBody)
+		{
+			b3Printf("Root link name = %s", u2b.getLinkName(u2b.getRootLinkIndex()).c_str());
+		}
+	}
+	return m_multiBody;
+}
+
+CommonExampleInterface* BlockSolverExampleCreateFunc(CommonExampleOptions& options)
+{
+	return new BlockSolverExample(options.m_guiHelper, options.m_option);
+}

examples/BlockSolver/BlockSolverExample.h

@@ -0,0 +1,19 @@
+
+#ifndef BLOCK_SOLVER_EXAMPLE_H
+#define BLOCK_SOLVER_EXAMPLE_H
+
+enum BlockSolverOptions
+{
+	BLOCK_SOLVER_SI=1<<0,
+	BLOCK_SOLVER_MLCP_PGS = 1 << 1,
+	BLOCK_SOLVER_MLCP_DANTZIG = 1 << 2,
+	BLOCK_SOLVER_BLOCK = 1 << 3,
+
+	BLOCK_SOLVER_SCENE_MB_STACK= 1 << 5,
+	BLOCK_SOLVER_SCENE_CHAIN = 1<< 6,
+	
+};
+
+class CommonExampleInterface* BlockSolverExampleCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //BLOCK_SOLVER_EXAMPLE_H

examples/BlockSolver/btBlockSolver.cpp

@@ -0,0 +1,160 @@
+#include "btBlockSolver.h"
+#include "BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h"
+#include "LinearMath/btQuickprof.h"
+
+struct btBlockSolverInternalData
+{
+	btAlignedObjectArray<btSolverBody> m_tmpSolverBodyPool;
+	btConstraintArray m_tmpSolverContactConstraintPool;
+	btConstraintArray m_tmpSolverNonContactConstraintPool;
+	btConstraintArray m_tmpSolverContactFrictionConstraintPool;
+	btConstraintArray m_tmpSolverContactRollingFrictionConstraintPool;
+
+	btAlignedObjectArray<int> m_orderTmpConstraintPool;
+	btAlignedObjectArray<int> m_orderNonContactConstraintPool;
+	btAlignedObjectArray<int> m_orderFrictionConstraintPool;
+	btAlignedObjectArray<btTypedConstraint::btConstraintInfo1> m_tmpConstraintSizesPool;
+	
+	
+	unsigned long m_btSeed2;
+	int m_fixedBodyId;
+	int m_maxOverrideNumSolverIterations;
+	btAlignedObjectArray<int> m_kinematicBodyUniqueIdToSolverBodyTable;  // only used for multithreading
+
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowGeneric;
+	btSingleConstraintRowSolver m_resolveSingleConstraintRowLowerLimit;
+	btSingleConstraintRowSolver m_resolveSplitPenetrationImpulse;
+
+	btBlockSolverInternalData()
+		:m_btSeed2(0),
+		m_fixedBodyId(-1),
+		m_maxOverrideNumSolverIterations(0),
+		m_resolveSingleConstraintRowGeneric(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverGeneric()),
+		m_resolveSingleConstraintRowLowerLimit(btSequentialImpulseConstraintSolver::getScalarConstraintRowSolverLowerLimit()),
+		m_resolveSplitPenetrationImpulse(btSequentialImpulseConstraintSolver::getScalarSplitPenetrationImpulseGeneric())
+	{
+	}
+};
+
+
+
+
+btBlockSolver::btBlockSolver()
+{
+	m_data = new btBlockSolverInternalData;
+}
+
+btBlockSolver::~btBlockSolver()
+{
+	delete m_data;
+}
+
+
+btScalar btBlockSolver::solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+
+	btSISolverSingleIterationData siData(m_data->m_tmpSolverBodyPool,
+		m_data->m_tmpSolverContactConstraintPool,
+		m_data->m_tmpSolverNonContactConstraintPool,
+		m_data->m_tmpSolverContactFrictionConstraintPool,
+		m_data->m_tmpSolverContactRollingFrictionConstraintPool,
+		m_data->m_orderTmpConstraintPool,
+		m_data->m_orderNonContactConstraintPool,
+		m_data->m_orderFrictionConstraintPool,
+		m_data->m_tmpConstraintSizesPool,
+		m_data->m_resolveSingleConstraintRowGeneric,
+		m_data->m_resolveSingleConstraintRowLowerLimit,
+		m_data->m_resolveSplitPenetrationImpulse,
+		m_data->m_kinematicBodyUniqueIdToSolverBodyTable,
+		m_data->m_btSeed2,
+		m_data->m_fixedBodyId,
+		m_data->m_maxOverrideNumSolverIterations);
+	
+	m_data->m_fixedBodyId = -1;
+	//todo: setup sse2/4 constraint row methods
+
+	btSequentialImpulseConstraintSolver::convertBodiesInternal(siData, bodies, numBodies, info);
+	btSequentialImpulseConstraintSolver::convertJointsInternal(siData, constraints, numConstraints, info);
+	
+	int i;
+	btPersistentManifold* manifold = 0;
+	//			btCollisionObject* colObj0=0,*colObj1=0;
+
+	for (i = 0; i < numManifolds; i++)
+	{
+		manifold = manifoldPtr[i];
+		btSequentialImpulseConstraintSolver::convertContactInternal(siData, manifold, info);
+	}
+
+
+
+	int numNonContactPool = siData.m_tmpSolverNonContactConstraintPool.size();
+	int numConstraintPool = siData.m_tmpSolverContactConstraintPool.size();
+	int numFrictionPool = siData.m_tmpSolverContactFrictionConstraintPool.size();
+
+	///@todo: use stack allocator for such temporarily memory, same for solver bodies/constraints
+	siData.m_orderNonContactConstraintPool.resizeNoInitialize(numNonContactPool);
+	if ((info.m_solverMode & SOLVER_USE_2_FRICTION_DIRECTIONS))
+		siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool * 2);
+	else
+		siData.m_orderTmpConstraintPool.resizeNoInitialize(numConstraintPool);
+
+	siData.m_orderFrictionConstraintPool.resizeNoInitialize(numFrictionPool);
+	{
+		int i;
+		for (i = 0; i < numNonContactPool; i++)
+		{
+			siData.m_orderNonContactConstraintPool[i] = i;
+		}
+		for (i = 0; i < numConstraintPool; i++)
+		{
+			siData.m_orderTmpConstraintPool[i] = i;
+		}
+		for (i = 0; i < numFrictionPool; i++)
+		{
+			siData.m_orderFrictionConstraintPool[i] = i;
+		}
+	}
+
+	btScalar leastSquaresResidual = 0;
+
+	
+
+	{
+		BT_PROFILE("solveGroupCacheFriendlyIterations");
+		///this is a special step to resolve penetrations (just for contacts)
+		btSequentialImpulseConstraintSolver::solveGroupCacheFriendlySplitImpulseIterationsInternal(siData, bodies, numBodies, manifoldPtr, numManifolds, constraints, numConstraints, info, debugDrawer);
+
+		int maxIterations = siData.m_maxOverrideNumSolverIterations > info.m_numIterations ? siData.m_maxOverrideNumSolverIterations : info.m_numIterations;
+
+		for (int iteration = 0; iteration < maxIterations; iteration++)
+		{
+			leastSquaresResidual = btSequentialImpulseConstraintSolver::solveSingleIterationInternal(siData, iteration, constraints, numConstraints, info);
+
+			if (leastSquaresResidual <= info.m_leastSquaresResidualThreshold || (iteration >= (maxIterations - 1)))
+			{
+#ifdef VERBOSE_RESIDUAL_PRINTF
+				printf("residual = %f at iteration #%d\n", m_leastSquaresResidual, iteration);
+#endif
+				break;
+			}
+		}
+	}
+
+	btScalar res = btSequentialImpulseConstraintSolver::solveGroupCacheFriendlyFinishInternal(siData, bodies, numBodies, info);
+	return res;
+}
+
+
+
+void btBlockSolver::solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher)
+{
+	btMultiBodyConstraintSolver::solveMultiBodyGroup(bodies, numBodies, manifold, numManifolds, constraints, numConstraints, multiBodyConstraints, numMultiBodyConstraints, info, debugDrawer, dispatcher);
+}
+
+void btBlockSolver::reset()
+{
+	//or just set m_data->m_btSeed2=0?
+	delete m_data;
+	m_data = new btBlockSolverInternalData;
+}

examples/BlockSolver/btBlockSolver.h

@@ -0,0 +1,29 @@
+#ifndef BT_BLOCK_SOLVER_H
+#define BT_BLOCK_SOLVER_H
+
+#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
+
+class btBlockSolver : public btMultiBodyConstraintSolver
+{
+	struct btBlockSolverInternalData* m_data;
+
+public:
+	btBlockSolver();
+	virtual ~btBlockSolver();
+
+	//btRigidBody
+	virtual btScalar solveGroup(btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, class btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+
+	//btMultibody
+	virtual void solveMultiBodyGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, btMultiBodyConstraint** multiBodyConstraints, int numMultiBodyConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
+
+	///clear internal cached data and reset random seed
+	virtual void reset();
+
+	virtual btConstraintSolverType getSolverType() const
+	{
+		return BT_BLOCK_SOLVER;
+	}
+};
+
+#endif //BT_BLOCK_SOLVER_H

examples/BulletRobotics/FixJointBoxes.cpp

@@ -0,0 +1,117 @@
+
+#include "FixJointBoxes.h"
+
+#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
+#include "Bullet3Common/b3AlignedObjectArray.h"
+#include "../CommonInterfaces/CommonRenderInterface.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "../CommonInterfaces/CommonGUIHelperInterface.h"
+#include "../SharedMemory/PhysicsServerSharedMemory.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/SharedMemoryPublic.h"
+#include "../CommonInterfaces/CommonParameterInterface.h"
+#include <string>
+#include <vector>
+#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
+
+class FixJointBoxes : public CommonExampleInterface
+{
+	GUIHelperInterface* m_guiHelper;
+	b3RobotSimulatorClientAPI m_robotSim;
+	int m_options;
+
+public:
+	FixJointBoxes(GUIHelperInterface* helper, int options)
+		: m_guiHelper(helper),
+		  m_options(options)
+	{
+	}
+
+	virtual ~FixJointBoxes()
+	{
+	}
+
+	virtual void physicsDebugDraw(int debugDrawMode)
+	{
+		m_robotSim.debugDraw(debugDrawMode);
+	}
+	virtual void initPhysics()
+	{
+		int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
+		m_robotSim.setGuiHelper(m_guiHelper);
+		bool connected = m_robotSim.connect(mode);
+
+		b3Printf("robotSim connected = %d", connected);
+
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_RGB_BUFFER_PREVIEW, 0);
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, 0);
+		m_robotSim.configureDebugVisualizer(COV_ENABLE_SEGMENTATION_MARK_PREVIEW, 0);
+
+		b3RobotSimulatorLoadUrdfFileArgs args;
+
+		size_t numCubes = 10;
+		std::vector<int> cubeIds(numCubes, 0);
+		for (int i = 0; i < numCubes; i++)
+		{
+			args.m_forceOverrideFixedBase = (i == 0);
+			args.m_startPosition.setValue(0, i * 0.05, 1);
+			cubeIds[i] = m_robotSim.loadURDF("cube_small.urdf", args);
+
+			b3JointInfo jointInfo;
+
+			jointInfo.m_parentFrame[1] = -0.025;
+			jointInfo.m_childFrame[1] = 0.025;
+			jointInfo.m_jointType = eFixedType;
+			// jointInfo.m_jointType = ePoint2PointType;
+			// jointInfo.m_jointType =	ePrismaticType;
+
+			if (i > 0)
+			{
+				m_robotSim.createConstraint(cubeIds[i], -1, cubeIds[i - 1], -1, &jointInfo);
+			}
+
+			m_robotSim.loadURDF("plane.urdf");
+		}
+	}
+
+	virtual void exitPhysics()
+	{
+		m_robotSim.disconnect();
+	}
+	virtual void stepSimulation(float deltaTime)
+	{
+		m_robotSim.stepSimulation();
+	}
+	virtual void renderScene()
+	{
+		m_robotSim.renderScene();
+	}
+
+	virtual bool mouseMoveCallback(float x, float y)
+	{
+		return m_robotSim.mouseMoveCallback(x, y);
+	}
+	virtual bool mouseButtonCallback(int button, int state, float x, float y)
+	{
+		return m_robotSim.mouseButtonCallback(button, state, x, y);
+	}
+	virtual bool keyboardCallback(int key, int state)
+	{
+		return false;
+	}
+
+	virtual void resetCamera()
+	{
+		// float dist = 1;
+		// float pitch = -20;
+		// float yaw = -30;
+		// float targetPos[3] = {0, 0.2, 0.5};
+
+		// m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
+	}
+};
+
+class CommonExampleInterface* FixJointBoxesCreateFunc(struct CommonExampleOptions& options)
+{
+	return new FixJointBoxes(options.m_guiHelper, options.m_option);
+}

examples/BulletRobotics/FixJointBoxes.h

@@ -0,0 +1,21 @@
+/*
+Bullet Continuous Collision Detection and Physics Library
+Copyright (c) 2016 Google Inc. http://bulletphysics.org
+
+This software is provided 'as-is', without any express or implied warranty.
+In no event will the authors be held liable for any damages arising from the use of this software.
+Permission is granted to anyone to use this software for any purpose, 
+including commercial applications, and to alter it and redistribute it freely, 
+subject to the following restrictions:
+
+1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
+2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
+3. This notice may not be removed or altered from any source distribution.
+*/
+
+#ifndef FIX_JOINT_BOXES_H
+#define FIX_JOINT_BOXES_H
+
+class CommonExampleInterface* FixJointBoxesCreateFunc(struct CommonExampleOptions& options);
+
+#endif  //FIX_JOINT_BOXES_H

examples/Evolution/NN3DWalkersTimeWarpBase.h

@@ -91,7 +91,7 @@ enum SolverEnumType
 	NNCGSOLVER = 2,
 	DANZIGSOLVER = 3,
 	LEMKESOLVER = 4,
-	FSSOLVER = 5,
+	
 	NUM_SOLVERS = 6
 };
 
@@ -103,7 +103,7 @@ static char GAUSSSEIDELSOLVER[] = "Gauss-Seidel Solver";
 static char NNCGSOLVER[] = "NNCG Solver";
 static char DANZIGSOLVER[] = "Danzig Solver";
 static char LEMKESOLVER[] = "Lemke Solver";
-static char FSSOLVER[] = "FeatherStone Solver";
+
 };  // namespace SolverType
 
 static const char* solverTypes[NUM_SOLVERS];
@@ -324,7 +324,7 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase
 		solverTypes[2] = SolverType::NNCGSOLVER;
 		solverTypes[3] = SolverType::DANZIGSOLVER;
 		solverTypes[4] = SolverType::LEMKESOLVER;
-		solverTypes[5] = SolverType::FSSOLVER;
+		
 
 		{
 			ComboBoxParams comboParams;
@@ -499,19 +499,12 @@ struct NN3DWalkersTimeWarpBase : public CommonRigidBodyBase
 				m_solver = new btMLCPSolver(mlcp);
 				break;
 			}
-			case FSSOLVER:
-			{
-				//			b3Printf("=%s=",SolverType::FSSOLVER);
-				//Use the btMultiBodyConstraintSolver for Featherstone btMultiBody support
-				m_solver = new btMultiBodyConstraintSolver;
-
-				break;
-			}
+			
 			default:
 				break;
 		}
 
-		if (SOLVER_TYPE != FSSOLVER)
+		if (1)
 		{
 			//TODO: Set parameters for other solvers
 

examples/ExampleBrowser/CMakeLists.txt

@@ -144,6 +144,7 @@ SET(ExtendedTutorialsSources
 )
 
 SET(BulletExampleBrowser_SRCS
+        ../BulletRobotics/FixJointBoxes.cpp
 
 	../TinyRenderer/geometry.cpp
 	../TinyRenderer/model.cpp
@@ -202,6 +203,10 @@ SET(BulletExampleBrowser_SRCS
 	../MultiThreadedDemo/MultiThreadedDemo.h
 	../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
 	../MultiThreadedDemo/CommonRigidBodyMTBase.h
+	../BlockSolver/btBlockSolver.cpp
+	../BlockSolver/btBlockSolver.h
+	../BlockSolver/BlockSolverExample.cpp
+	../BlockSolver/BlockSolverExample.h
 	../Tutorial/Tutorial.cpp
 	../Tutorial/Tutorial.h
 	../Tutorial/Dof6ConstraintTutorial.cpp
@@ -333,7 +338,6 @@ SET(BulletExampleBrowser_SRCS
   ../MultiBody/InvertedPendulumPDControl.cpp
   ../MultiBody/InvertedPendulumPDControl.h
  ../MultiBody/MultiBodyConstraintFeedback.cpp
-  ../MultiBody/SerialChains.cpp
   ../MultiBody/MultiDofDemo.cpp
   ../MultiBody/MultiDofDemo.h
   ../RigidBody/RigidBodySoftContact.cpp

examples/ExampleBrowser/ExampleEntries.cpp

@@ -1,7 +1,9 @@
 #include "ExampleEntries.h"
 
+#include "../BlockSolver/BlockSolverExample.h"
 #include "LinearMath/btAlignedObjectArray.h"
 #include "EmptyExample.h"
+#include "../BulletRobotics/FixJointBoxes.h"
 #include "../RenderingExamples/RenderInstancingDemo.h"
 #include "../RenderingExamples/CoordinateSystemDemo.h"
 #include "../RenderingExamples/RaytracerSetup.h"
@@ -29,7 +31,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"
@@ -128,6 +130,10 @@ static ExampleEntry gDefaultExamples[] =
 
 		ExampleEntry(1, "Soft Contact", "Using the error correction parameter (ERP) and constraint force mixing (CFM) values for contacts to simulate compliant contact.", RigidBodySoftContactCreateFunc),
 
+
+		ExampleEntry(0, "Bullet Robotics"),
+		ExampleEntry(1, "FixJoint Boxes", "FixJoint Boxes", FixJointBoxesCreateFunc),
+
 		ExampleEntry(0, "MultiBody"),
 		ExampleEntry(1, "MultiDof", "Create a basic btMultiBody with 3-DOF spherical joints (mobilizers). The demo uses a fixed base or a floating base at restart.", MultiDofCreateFunc),
 		ExampleEntry(1, "TestJointTorque", "Apply a torque to a btMultiBody with 1-DOF joints (mobilizers). This setup is similar to API/TestHingeTorque.", TestJointTorqueCreateFunc),
@@ -136,8 +142,8 @@ 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.",
 					 PhysicsServerCreateFuncBullet2),
@@ -150,6 +156,13 @@ static ExampleEntry gDefaultExamples[] =
 		//
 		//	ExampleEntry(1, "Physics Client (Direct)", "Create a physics client that can communicate with a physics server directly in-process.", PhysicsClientCreateFunc,eCLIENTEXAMPLE_DIRECT),
 
+
+		ExampleEntry(0, "BlockSolver"),
+		ExampleEntry(1, "Stack MultiBody SI", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK+ BLOCK_SOLVER_SI),
+		ExampleEntry(1, "Stack MultiBody MLCP PGS", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_PGS),
+		ExampleEntry(1, "Stack MultiBody MLCP Dantzig", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_MLCP_DANTZIG),
+		ExampleEntry(1, "Stack MultiBody Block", "Create a stack of blocks, with heavy block at the top", BlockSolverExampleCreateFunc, BLOCK_SOLVER_SCENE_MB_STACK + BLOCK_SOLVER_BLOCK),
+
 		ExampleEntry(0, "Inverse Dynamics"),
 		ExampleEntry(1, "Inverse Dynamics URDF", "Create a btMultiBody from URDF. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_LOAD_URDF),
 		ExampleEntry(1, "Inverse Dynamics Prog", "Create a btMultiBody programatically. Create an inverse MultiBodyTree model from that. Use either decoupled PD control or computed torque control using the inverse model to track joint position targets", InverseDynamicsExampleCreateFunc, BT_ID_PROGRAMMATICALLY),

examples/ExampleBrowser/premake4.lua

@@ -162,6 +162,7 @@ project "App_BulletExampleBrowser"
 		"../Evolution/NN3DWalkers.h",
 		"../Collision/*",
 		"../RoboticsLearning/*",
+		"../BlockSolver/*",
 		"../Collision/Internal/*",
 		"../Benchmarks/*",
 		"../MultiThreadedDemo/*",