Bart/bullet3
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

prepare small experiment with block solver

Browse Source
This commit is contained in:
erwincoumans
2019-02-26 23:27:05 -08:00
parent c44471c38c
commit d7e087de16
20 changed files with 2877 additions and 800 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
examples/BlockSolver/BlockSolverExample.cpp
View File

@@ -115,7 +115,7 @@ void BlockSolverExample::initPhysics()
}
if (m_option&BLOCK_SOLVER_BLOCK)
{
//m_solver = new btBlockSolver();
m_solver = new btBlockSolver();
}
btAssert(m_solver);

2
examples/BlockSolver/btBlockSolver.h
View File

@@ -3,7 +3,7 @@
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
class btBlockSolver : public btConstraintSolver
class btBlockSolver : public btMultiBodyConstraintSolver
{
struct btBlockSolverInternalData* m_data;

335
examples/ConstraintSolvers/BoxStacks.cpp Normal file
View File

@@ -0,0 +1,335 @@
#include "BoxStacks.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class BoxStacks : public CommonMultiBodyBase
{
public:
BoxStacks(GUIHelperInterface* helper);
virtual ~BoxStacks();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 1;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY);
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false);
void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55));
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
};
static bool g_fixedBase = true;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
static int g_constraintSolverType = 0;
BoxStacks::BoxStacks(GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
m_guiHelper->setUpAxis(1);
}
BoxStacks::~BoxStacks()
{
// Do nothing
}
void BoxStacks::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void BoxStacks::initPhysics()
{
m_guiHelper->setUpAxis(1);
if (g_firstInit)
{
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
g_firstInit = false;
}
///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();
if (g_constraintSolverType == 3)
{
g_constraintSolverType = 0;
g_fixedBase = !g_fixedBase;
}
btMLCPSolverInterface* mlcp;
switch (g_constraintSolverType++)
{
case 0:
m_solver = new btMultiBodyConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
break;
case 1:
mlcp = new btSolveProjectedGaussSeidel();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
break;
default:
mlcp = new btDantzigSolver();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Dantzig");
break;
}
m_solver = new btMultiBodyBlockConstraintSolver();
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld = world;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0)));
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
/// Create a few basic rigid bodies
btVector3 groundHalfExtents(50, 50, 50);
btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 00));
btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
createBoxStack(1, 0, 0);
btScalar groundHeight = btScalar(-51.55);
btScalar mass = btScalar(0.0);
btVector3 localInertia(0, 0, 0);
groundShape->calculateLocalInertia(mass, localInertia);
// Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, groundHeight, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
// Add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);
createGround();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void BoxStacks::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ)
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
const btScalar boxHalfSize = btScalar(0.1);
btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.0);
btVector3 localInertia(0, 0, 0);
colShape->calculateLocalInertia(mass,localInertia);
for (int i = 0; i < numBoxes; ++i)
{
startTransform.setOrigin(btVector3(centerX, 1+btScalar(btScalar(2) * boxHalfSize * i), centerZ));
createRigidBody(mass, startTransform, colShape);
}
}
btMultiBody* BoxStacks::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void BoxStacks::createGround(const btVector3& halfExtents, btScalar zOffSet)
{
btCollisionShape* groundShape = new btBoxShape(halfExtents);
m_collisionShapes.push_back(groundShape);
// rigidbody is dynamic if and only if mass is non zero, otherwise static
btScalar mass(0.);
const bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
// using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
// add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);
}
void BoxStacks::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
CommonExampleInterface* BoxStacksCreateFunc(CommonExampleOptions& options)
{
return new BoxStacks(options.m_guiHelper);
}

7
examples/ConstraintSolvers/BoxStacks.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H
#define CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H
class CommonExampleInterface* BoxStacksCreateFunc(struct CommonExampleOptions& options);
#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H

224
examples/ConstraintSolvers/BoxStacks_MLCP.cpp Normal file
View File

@@ -0,0 +1,224 @@
#include "BoxStacks_MLCP.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "../Importers/ImportURDFDemo/BulletUrdfImporter.h"
#include "../Importers/ImportURDFDemo/MyMultiBodyCreator.h"
#include "../Importers/ImportURDFDemo/URDF2Bullet.h"
#include "BulletDynamics/MLCPSolvers/btLemkeSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
class BoxStacks_MLCP : public CommonMultiBodyBase
{
public:
BoxStacks_MLCP(GUIHelperInterface* helper);
virtual ~BoxStacks_MLCP();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 2;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {0, 0, 0};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
btMultiBody* createMultiBody(btScalar mass, const btTransform& trans, btCollisionShape* collisionShape);
btMultiBody* loadRobot(std::string filepath = "kuka_iiwa/model.urdf");
};
static int g_constraintSolverType = 0;
BoxStacks_MLCP::BoxStacks_MLCP(GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
}
BoxStacks_MLCP::~BoxStacks_MLCP()
{
// Do nothing
}
void BoxStacks_MLCP::stepSimulation(float deltaTime)
{
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
for (int i = 0; i < m_dynamicsWorld->getNumMultibodies(); i++)
{
btVector3 pos = m_dynamicsWorld->getMultiBody(i)->getBaseWorldTransform().getOrigin();
printf("pos[%d]=%f,%f,%f\n", i, pos.x(), pos.y(), pos.z());
}
}
void BoxStacks_MLCP::initPhysics()
{
m_guiHelper->setUpAxis(2);
createEmptyDynamicsWorld();
m_dynamicsWorld->getSolverInfo().m_numIterations = 50;
if (g_constraintSolverType == 5)
{
g_constraintSolverType = 0;
}
btMultiBodyConstraintSolver* sol = 0;
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;
case 3:
mlcp = new btLemkeSolver();
sol = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Lemke");
break;
default:
sol = new btMultiBodyBlockConstraintSolver();
b3Printf("btMultiBodyBlockConstraintSolver");
break;
}
m_solver = sol;
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, sol, m_collisionConfiguration);
m_dynamicsWorld = world;
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4);
m_dynamicsWorld->setGravity(btVector3(0,0,-10));
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
if (m_dynamicsWorld->getDebugDrawer())
m_dynamicsWorld->getDebugDrawer()->setDebugMode(btIDebugDraw::DBG_DrawWireframe+btIDebugDraw::DBG_DrawContactPoints);
///create a few basic rigid bodies
bool loadPlaneFromURDF = true;
if (loadPlaneFromURDF)
{
loadRobot("plane.urdf");
} 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);
}
{
btBoxShape* boxShape = createBoxShape(btVector3(btScalar(.1), btScalar(.1), btScalar(.1)));
m_collisionShapes.push_back(boxShape);
btScalar mass = 10;
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 0.5));
btMultiBody* body = createMultiBody(mass, tr, boxShape);
}
{
btMultiBody* mb = loadRobot("cube_small.urdf");
btTransform tr;
tr.setIdentity();
tr.setOrigin(btVector3(0, 0, 1.));
mb->setBaseWorldTransform(tr);
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
btMultiBody* BoxStacks_MLCP::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*BoxStacks_MLCP::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* BoxStacks_MLCPCreateFunc(CommonExampleOptions& options)
{
return new BoxStacks_MLCP(options.m_guiHelper);
}

7
examples/ConstraintSolvers/BoxStacks_MLCP.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H
#define CONSTRAINT_SOLVERS_BOX_STACKS_MLCP_DEMO_H
class CommonExampleInterface* BoxStacks_MLCPCreateFunc(struct CommonExampleOptions& options);
#endif // CONSTRAINT_SOLVERS_BOX_STACKS_DEMO_H

335
examples/ConstraintSolvers/Grasp_Block.cpp Normal file
View File

@@ -0,0 +1,335 @@
#include "Grasp_Block.h"
#include "../OpenGLWindow/SimpleOpenGL3App.h"
#include "btBulletDynamicsCommon.h"
#include "BulletDynamics/MLCPSolvers/btDantzigSolver.h"
#include "BulletDynamics/MLCPSolvers/btSolveProjectedGaussSeidel.h"
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
#include "BulletDynamics/Featherstone/btMultiBodyLink.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointLimitConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodyJointMotor.h"
#include "BulletDynamics/Featherstone/btMultiBodyPoint2Point.h"
#include "BulletDynamics/Featherstone/btMultiBodyFixedConstraint.h"
#include "BulletDynamics/Featherstone/btMultiBodySliderConstraint.h"
#include "../OpenGLWindow/GLInstancingRenderer.h"
#include "BulletCollision/CollisionShapes/btShapeHull.h"
#include "../CommonInterfaces/CommonMultiBodyBase.h"
class Grasp_Block : public CommonMultiBodyBase
{
public:
Grasp_Block(GUIHelperInterface* helper);
virtual ~Grasp_Block();
virtual void initPhysics();
virtual void stepSimulation(float deltaTime);
virtual void resetCamera()
{
float dist = 1;
float pitch = -35;
float yaw = 50;
float targetPos[3] = {-3, 2.8, -2.5};
m_guiHelper->resetCamera(dist, yaw, pitch, targetPos[0], targetPos[1], targetPos[2]);
}
void createBoxStack(int numBoxes, btScalar centerX, btScalar centerY);
btMultiBody* createFeatherstoneMultiBody(class btMultiBodyDynamicsWorld* world, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical = false, bool fixedBase = false);
void createGround(const btVector3& halfExtents = btVector3(50, 50, 50), btScalar zOffSet = btScalar(-1.55));
void addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents);
};
static bool g_fixedBase = true;
static bool g_firstInit = true;
static float scaling = 0.4f;
static float friction = 1.;
static int g_constraintSolverType = 0;
Grasp_Block::Grasp_Block(GUIHelperInterface* helper)
: CommonMultiBodyBase(helper)
{
m_guiHelper->setUpAxis(1);
}
Grasp_Block::~Grasp_Block()
{
// Do nothing
}
void Grasp_Block::stepSimulation(float deltaTime)
{
//use a smaller internal timestep, there are stability issues
float internalTimeStep = 1. / 240.f;
m_dynamicsWorld->stepSimulation(deltaTime, 10, internalTimeStep);
}
void Grasp_Block::initPhysics()
{
m_guiHelper->setUpAxis(1);
if (g_firstInit)
{
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraDistance(btScalar(10. * scaling));
m_guiHelper->getRenderInterface()->getActiveCamera()->setCameraPitch(50);
g_firstInit = false;
}
///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();
if (g_constraintSolverType == 3)
{
g_constraintSolverType = 0;
g_fixedBase = !g_fixedBase;
}
btMLCPSolverInterface* mlcp;
switch (g_constraintSolverType++)
{
case 0:
m_solver = new btMultiBodyConstraintSolver;
b3Printf("Constraint Solver: Sequential Impulse");
break;
case 1:
mlcp = new btSolveProjectedGaussSeidel();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + PGS");
break;
default:
mlcp = new btDantzigSolver();
m_solver = new btMultiBodyMLCPConstraintSolver(mlcp);
b3Printf("Constraint Solver: MLCP + Dantzig");
break;
}
m_solver = new btMultiBodyBlockConstraintSolver();
btMultiBodyDynamicsWorld* world = new btMultiBodyDynamicsWorld(m_dispatcher, m_broadphase, m_solver, m_collisionConfiguration);
m_dynamicsWorld = world;
m_guiHelper->createPhysicsDebugDrawer(m_dynamicsWorld);
m_dynamicsWorld->setGravity(btVector3(btScalar(0), btScalar(-9.81), btScalar(0)));
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
/// Create a few basic rigid bodies
btVector3 groundHalfExtents(50, 50, 50);
btCollisionShape* groundShape = new btBoxShape(groundHalfExtents);
m_collisionShapes.push_back(groundShape);
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -50, 00));
btVector3 linkHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
btVector3 baseHalfExtents(btScalar(0.05), btScalar(0.37), btScalar(0.1));
// createBoxStack(5, 0, 0);
btScalar groundHeight = btScalar(-51.55);
btScalar mass = btScalar(0.0);
btVector3 localInertia(0, 0, 0);
groundShape->calculateLocalInertia(mass, localInertia);
// Using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, groundHeight, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
// Add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);
createGround();
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
}
void Grasp_Block::createBoxStack(int numBoxes, btScalar centerX, btScalar centerZ)
{
//create a few dynamic rigidbodies
// Re-using the same collision is better for memory usage and performance
const btScalar boxHalfSize = btScalar(0.1);
btBoxShape* colShape = createBoxShape(btVector3(boxHalfSize, boxHalfSize, boxHalfSize));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.0);
btVector3 localInertia(0, 0, 0);
colShape->calculateLocalInertia(mass,localInertia);
for (int i = 0; i < numBoxes; ++i)
{
startTransform.setOrigin(btVector3(centerX, btScalar(btScalar(2) * boxHalfSize * i), centerZ));
createRigidBody(mass, startTransform, colShape);
}
}
btMultiBody* Grasp_Block::createFeatherstoneMultiBody(btMultiBodyDynamicsWorld* pWorld, int numLinks, const btVector3& basePosition, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents, bool spherical, bool fixedBase)
{
//init the base
btVector3 baseInertiaDiag(0.f, 0.f, 0.f);
float baseMass = 1.f;
if (baseMass)
{
btCollisionShape* pTempBox = new btBoxShape(btVector3(baseHalfExtents[0], baseHalfExtents[1], baseHalfExtents[2]));
pTempBox->calculateLocalInertia(baseMass, baseInertiaDiag);
delete pTempBox;
}
bool canSleep = false;
btMultiBody* pMultiBody = new btMultiBody(numLinks, baseMass, baseInertiaDiag, fixedBase, canSleep);
btQuaternion baseOriQuat(0.f, 0.f, 0.f, 1.f);
pMultiBody->setBasePos(basePosition);
pMultiBody->setWorldToBaseRot(baseOriQuat);
btVector3 vel(0, 0, 0);
//init the links
btVector3 hingeJointAxis(1, 0, 0);
float linkMass = 1.f;
btVector3 linkInertiaDiag(0.f, 0.f, 0.f);
btCollisionShape* pTempBox = new btBoxShape(btVector3(linkHalfExtents[0], linkHalfExtents[1], linkHalfExtents[2]));
pTempBox->calculateLocalInertia(linkMass, linkInertiaDiag);
delete pTempBox;
//y-axis assumed up
btVector3 parentComToCurrentCom(0, -linkHalfExtents[1] * 2.f, 0); //par body's COM to cur body's COM offset
btVector3 currentPivotToCurrentCom(0, -linkHalfExtents[1], 0); //cur body's COM to cur body's PIV offset
btVector3 parentComToCurrentPivot = parentComToCurrentCom - currentPivotToCurrentCom; //par body's COM to cur body's PIV offset
//////
btScalar q0 = 0.f * SIMD_PI / 180.f;
btQuaternion quat0(btVector3(0, 1, 0).normalized(), q0);
quat0.normalize();
/////
for (int i = 0; i < numLinks; ++i)
{
if (!spherical)
pMultiBody->setupRevolute(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), hingeJointAxis, parentComToCurrentPivot, currentPivotToCurrentCom, true);
else
//pMultiBody->setupPlanar(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f)/*quat0*/, btVector3(1, 0, 0), parentComToCurrentPivot*2, false);
pMultiBody->setupSpherical(i, linkMass, linkInertiaDiag, i - 1, btQuaternion(0.f, 0.f, 0.f, 1.f), parentComToCurrentPivot, currentPivotToCurrentCom, true);
}
pMultiBody->finalizeMultiDof();
///
pWorld->addMultiBody(pMultiBody);
///
return pMultiBody;
}
void Grasp_Block::createGround(const btVector3& halfExtents, btScalar zOffSet)
{
btCollisionShape* groundShape = new btBoxShape(halfExtents);
m_collisionShapes.push_back(groundShape);
// rigidbody is dynamic if and only if mass is non zero, otherwise static
btScalar mass(0.);
const bool isDynamic = (mass != 0.f);
btVector3 localInertia(0, 0, 0);
if (isDynamic)
groundShape->calculateLocalInertia(mass, localInertia);
// using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btTransform groundTransform;
groundTransform.setIdentity();
groundTransform.setOrigin(btVector3(0, -halfExtents.z() + zOffSet, 0));
btDefaultMotionState* myMotionState = new btDefaultMotionState(groundTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass, myMotionState, groundShape, localInertia);
btRigidBody* body = new btRigidBody(rbInfo);
// add the body to the dynamics world
m_dynamicsWorld->addRigidBody(body, 1, 1 + 2);
}
void Grasp_Block::addColliders(btMultiBody* pMultiBody, btMultiBodyDynamicsWorld* pWorld, const btVector3& baseHalfExtents, const btVector3& linkHalfExtents)
{
btAlignedObjectArray<btQuaternion> world_to_local;
world_to_local.resize(pMultiBody->getNumLinks() + 1);
btAlignedObjectArray<btVector3> local_origin;
local_origin.resize(pMultiBody->getNumLinks() + 1);
world_to_local[0] = pMultiBody->getWorldToBaseRot();
local_origin[0] = pMultiBody->getBasePos();
{
btScalar quat[4] = {-world_to_local[0].x(), -world_to_local[0].y(), -world_to_local[0].z(), world_to_local[0].w()};
if (1)
{
btCollisionShape* box = new btBoxShape(baseHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, -1);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(local_origin[0]);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
pWorld->addCollisionObject(col, 2, 1 + 2);
col->setFriction(friction);
pMultiBody->setBaseCollider(col);
}
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
const int parent = pMultiBody->getParent(i);
world_to_local[i + 1] = pMultiBody->getParentToLocalRot(i) * world_to_local[parent + 1];
local_origin[i + 1] = local_origin[parent + 1] + (quatRotate(world_to_local[i + 1].inverse(), pMultiBody->getRVector(i)));
}
for (int i = 0; i < pMultiBody->getNumLinks(); ++i)
{
btVector3 posr = local_origin[i + 1];
btScalar quat[4] = {-world_to_local[i + 1].x(), -world_to_local[i + 1].y(), -world_to_local[i + 1].z(), world_to_local[i + 1].w()};
btCollisionShape* box = new btBoxShape(linkHalfExtents);
btMultiBodyLinkCollider* col = new btMultiBodyLinkCollider(pMultiBody, i);
col->setCollisionShape(box);
btTransform tr;
tr.setIdentity();
tr.setOrigin(posr);
tr.setRotation(btQuaternion(quat[0], quat[1], quat[2], quat[3]));
col->setWorldTransform(tr);
col->setFriction(friction);
pWorld->addCollisionObject(col, 2, 1 + 2);
pMultiBody->getLink(i).m_collider = col;
}
}
CommonExampleInterface* Grasp_BlockCreateFunc(CommonExampleOptions& options)
{
return new Grasp_Block(options.m_guiHelper);
}

7
examples/ConstraintSolvers/Grasp_Block.h Normal file
View File

@@ -0,0 +1,7 @@
#ifndef CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H
#define CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H
class CommonExampleInterface* Grasp_BlockCreateFunc(struct CommonExampleOptions& options);
#endif // CONSTRAINT_SOLVERS_GRASP_BLOCK_DEMO_H

51
examples/MultiBody/SerialChains.cpp → examples/ConstraintSolvers/SerialChains.cpp
View File

@@ -7,6 +7,7 @@
#include "BulletDynamics/Featherstone/btMultiBody.h"
#include "BulletDynamics/Featherstone/btMultiBodyConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyBlockConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h"
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#include "BulletDynamics/Featherstone/btMultiBodyLinkCollider.h"
@@ -112,12 +113,13 @@ void SerialChains::initPhysics()
b3Printf("Constraint Solver: MLCP + Dantzig");
break;
}
m_solver = new btMultiBodyBlockConstraintSolver();
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, -10, 0));
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
m_dynamicsWorld->getSolverInfo().m_globalCfm = btScalar(1e-4); //todo: what value is good?
///create a few basic rigid bodies
btVector3 groundHalfExtents(50, 50, 50);
@@ -237,6 +239,53 @@ void SerialChains::initPhysics()
createGround();
{
btVector3 halfExtents(.5,.5,.5);
btBoxShape* colShape = new btBoxShape(halfExtents);
//btCollisionShape* colShape = new btSphereShape(btScalar(1.));
m_collisionShapes.push_back(colShape);
/// Create Dynamic Objects
btTransform startTransform;
startTransform.setIdentity();
btScalar mass(1.f);
//rigidbody is dynamic if and only if mass is non zero, otherwise static
bool isDynamic = (mass != 0.f);
btVector3 localInertia(0,0,0);
if (isDynamic)
colShape->calculateLocalInertia(mass,localInertia);
startTransform.setOrigin(btVector3(
btScalar(0.0),
0.0,
btScalar(0.0)));
//using motionstate is recommended, it provides interpolation capabilities, and only synchronizes 'active' objects
btDefaultMotionState* myMotionState = new btDefaultMotionState(startTransform);
btRigidBody::btRigidBodyConstructionInfo rbInfo(mass,myMotionState,colShape,localInertia);
// btRigidBody* body = new btRigidBody(rbInfo);
// m_dynamicsWorld->addRigidBody(body);//,1,1+2);
{
btVector3 pointInA = -linkHalfExtents;
// btVector3 pointInB = halfExtents;
btMatrix3x3 frameInA;
btMatrix3x3 frameInB;
frameInA.setIdentity();
frameInB.setIdentity();
btVector3 jointAxis(1.0,0.0,0.0);
//btMultiBodySliderConstraint* p2p = new btMultiBodySliderConstraint(mbC,numLinks-1,body,pointInA,pointInB,frameInA,frameInB,jointAxis);
btMultiBodyPoint2Point* p2p = new btMultiBodyPoint2Point(mbC1, numLinks- 1 , mbC2, numLinks - 1, pointInA, pointInA);
p2p->setMaxAppliedImpulse(20.0);
m_dynamicsWorld->addMultiBodyConstraint(p2p);
}
}
m_guiHelper->autogenerateGraphicsObjects(m_dynamicsWorld);
/////////////////////////////////////////////////////////////////

0
examples/MultiBody/SerialChains.h → examples/ConstraintSolvers/SerialChains.h
View File

5
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -202,6 +202,10 @@ SET(BulletExampleBrowser_SRCS
../MultiThreadedDemo/MultiThreadedDemo.h
../MultiThreadedDemo/CommonRigidBodyMTBase.cpp
../MultiThreadedDemo/CommonRigidBodyMTBase.h
../ConstraintSolvers/BoxStacks.cpp
../ConstraintSolvers/BoxStacks_MLCP.cpp
../ConstraintSolvers/Grasp_Block.cpp
../ConstraintSolvers/SerialChains.cpp
../BlockSolver/btBlockSolver.cpp
../BlockSolver/btBlockSolver.h
../BlockSolver/BlockSolverExample.cpp
@@ -337,7 +341,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

12
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -21,6 +21,10 @@
#include "../Importers/ImportSDFDemo/ImportSDFSetup.h"
#include "../Importers/ImportMJCFDemo/ImportMJCFSetup.h"
#include "../Collision/CollisionTutorialBullet2.h"
#include "../ConstraintSolvers/SerialChains.h"
#include "../ConstraintSolvers/BoxStacks.h"
#include "../ConstraintSolvers/BoxStacks_MLCP.h"
#include "../ConstraintSolvers/Grasp_Block.h"
#include "../GyroscopicDemo/GyroscopicSetup.h"
#include "../Constraints/Dof6Spring2Setup.h"
#include "../Constraints/ConstraintPhysicsSetup.h"
@@ -30,7 +34,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,7 +141,13 @@ 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(0, "Constraint Solvers"),
ExampleEntry(1, "Serial Chains", "Show colliding two serial chains using different constraint solvers.", SerialChainsCreateFunc, 0),
ExampleEntry(1, "Box Stack", "Show box stacks with different constraint solvers for each stack.", BoxStacksCreateFunc, 0),
ExampleEntry(1, "Box Stack MLCP", "Show box stacks with different constraint solvers for each stack.", BoxStacks_MLCPCreateFunc, 0),
ExampleEntry(1, "Grasp Block", "Show box stacks with different constraint solvers for each stack.", Grasp_BlockCreateFunc, 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.",

1
examples/ExampleBrowser/premake4.lua
View File

@@ -163,6 +163,7 @@ project "App_BulletExampleBrowser"
"../Collision/*",
"../RoboticsLearning/*",
"../BlockSolver/*",
"../ConstraintSolvers/*",
"../Collision/Internal/*",
"../Benchmarks/*",
"../MultiThreadedDemo/*",