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

Merge pull request #2 from bulletphysics/master

Browse Source 
sync with upstream
This commit is contained in:
fuchuyuan
2019-03-07 17:03:33 -08:00
committed by GitHub
parent 1dbbccd4a9 ba61c0911f
commit b347d9f667
27 changed files with 1739 additions and 313 deletions
Download Patch File Download Diff File Expand all files Collapse all files

219
examples/BlockSolver/BlockSolverExample.cpp Normal file
View File

@@ -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);
}

19
examples/BlockSolver/BlockSolverExample.h Normal file
View File

@@ -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

160
examples/BlockSolver/btBlockSolver.cpp Normal file
View File

@@ -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;
}

29
examples/BlockSolver/btBlockSolver.h Normal file
View File

@@ -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

117
examples/BulletRobotics/FixJointBoxes.cpp Normal file
View File

@@ -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);
}

21
examples/BulletRobotics/FixJointBoxes.h Normal file
View File

@@ -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

17
examples/Evolution/NN3DWalkersTimeWarpBase.h
View File

@@ -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

6
examples/ExampleBrowser/CMakeLists.txt
View File

@@ -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

19
examples/ExampleBrowser/ExampleEntries.cpp
View File

@@ -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),

1
examples/ExampleBrowser/premake4.lua
View File

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

147
examples/RobotSimulator/MinitaurSimulatorExample.cpp Normal file
View File

@@ -0,0 +1,147 @@
#include "MinitaurSimulatorExample.h"
#include "MinitaurSetup.h"
#include "../CommonInterfaces/CommonGraphicsAppInterface.h"
#include "Bullet3Common/b3Quaternion.h"
#include "Bullet3Common/b3AlignedObjectArray.h"
#include "../CommonInterfaces/CommonRenderInterface.h"
#include "../CommonInterfaces/CommonExampleInterface.h"
#include "../CommonInterfaces/CommonGUIHelperInterface.h"
#include "../SharedMemory/PhysicsServerSharedMemory.h"
#include "../SharedMemory/SharedMemoryPublic.h"
#include "../CommonInterfaces/CommonParameterInterface.h"
#include "../SharedMemory/PhysicsClientC_API.h"
#include <string>
#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
#include "../Utils/b3Clock.h"
///quick demo showing the right-handed coordinate system and positive rotations around each axis
class MinitaurSimulatorExample : public CommonExampleInterface
{
CommonGraphicsApp* m_app;
GUIHelperInterface* m_guiHelper;
b3RobotSimulatorClientAPI m_robotSim;
int m_options;
double m_time;
double m_gravityAccelerationZ;
MinitaurSetup m_minitaur;
int m_minitaurUid;
public:
MinitaurSimulatorExample(GUIHelperInterface* helper, int options)
: m_app(helper->getAppInterface()),
m_guiHelper(helper),
m_options(options),
m_gravityAccelerationZ(-10),
m_minitaurUid(-1)
{
m_app->setUpAxis(2);
}
virtual ~MinitaurSimulatorExample()
{
}
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);
//hide the perception camera views for rbd, depth and segmentation mask
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);
b3Printf("robotSim connected = %d", connected);
SliderParams slider("Gravity", &m_gravityAccelerationZ);
slider.m_minVal = -10;
slider.m_maxVal = 10;
m_guiHelper->getParameterInterface()->registerSliderFloatParameter(slider);
//when in the debugger, don't crash when a command isn't processed immediately, give it 10 seconds
m_robotSim.setTimeOut(10);
m_robotSim.loadURDF("plane.urdf");
m_minitaurUid = m_minitaur.setupMinitaur(&m_robotSim, btVector3(0, 0, .3));
{
b3RobotSimulatorLoadUrdfFileArgs args;
args.m_startPosition.setValue(0, 0, 1);
args.m_startOrientation.setEulerZYX(0, 0, 0);
args.m_useMultiBody = true;
m_robotSim.loadURDF("cube_small.urdf", args);
}
}
virtual void exitPhysics()
{
m_robotSim.disconnect();
}
virtual void stepSimulation(float deltaTime)
{
m_robotSim.setGravity(btVector3(0, 0, m_gravityAccelerationZ));
m_robotSim.stepSimulation();
for (int i = 0; i < m_robotSim.getNumJoints(m_minitaurUid);i++)
{
b3JointSensorState state;
m_robotSim.getJointState(this->m_minitaurUid, i, &state);
}
b3JointStates2 states;
m_robotSim.getJointStates(m_minitaurUid, states);
}
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.5;
float pitch = -10;
float yaw = 18;
float targetPos[3] = {-0.2, 0.8, 0.3};
if (m_app->m_renderer && m_app->m_renderer->getActiveCamera())
{
m_app->m_renderer->getActiveCamera()->setCameraDistance(dist);
m_app->m_renderer->getActiveCamera()->setCameraPitch(pitch);
m_app->m_renderer->getActiveCamera()->setCameraYaw(yaw);
m_app->m_renderer->getActiveCamera()->setCameraTargetPosition(targetPos[0], targetPos[1], targetPos[2]);
}
}
};
class CommonExampleInterface* MinitaurSimulatorExampleCreateFunc(struct CommonExampleOptions& options)
{
return new MinitaurSimulatorExample(options.m_guiHelper, options.m_option);
}

22
examples/RobotSimulator/MinitaurSimulatorExample.h Normal file
View File

@@ -0,0 +1,22 @@
/*
Bullet Continuous Collision Detection and Physics Library
Copyright (c) 2019 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 MINITAUR_SIMULATOR_EXAMPLE_H
#define MINITAUR_SIMULATOR_EXAMPLE_H
class CommonExampleInterface* MinitaurSimulatorExampleCreateFunc(struct CommonExampleOptions& options);
#endif //MINITAUR_SIMULATOR_EXAMPLE_H

4
examples/RoboticsLearning/GripperGraspExample.cpp
View File

@@ -62,6 +62,10 @@ public:
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
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);
b3Printf("robotSim connected = %d", connected);
if ((m_options & eGRIPPER_GRASP) != 0)

3
examples/RoboticsLearning/KukaGraspExample.cpp
View File

@@ -72,6 +72,9 @@ public:
int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
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);
// 0;//m_robotSim.connect(m_guiHelper);
b3Printf("robotSim connected = %d", connected);

3
examples/RoboticsLearning/R2D2GraspExample.cpp
View File

@@ -51,6 +51,9 @@ public:
int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
m_robotSim.setGuiHelper(m_guiHelper);
bool connected = m_robotSim.connect(mode);
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);
b3Printf("robotSim connected = %d", connected);

49
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -990,6 +990,10 @@ B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMe
B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState* state)
{
const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle;
b3Assert(status);
int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId;
@@ -997,13 +1001,14 @@ B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemo
if (bodyIndex >= 0)
{
b3JointInfo info;
bool result = b3GetJointInfo(physClient, bodyIndex, jointIndex, &info) != 0;
if (result)
if (result && status->m_sendActualStateArgs.m_stateDetails)
{
if ((info.m_qIndex >= 0) && (info.m_uIndex >= 0) && (info.m_qIndex < MAX_DEGREE_OF_FREEDOM) && (info.m_uIndex < MAX_DEGREE_OF_FREEDOM))
{
state->m_jointPosition = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex];
state->m_jointPosition = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQ[info.m_qIndex];
state->m_jointVelocity = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[info.m_uIndex];
}
else
{
@@ -1012,9 +1017,9 @@ B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemo
}
for (int ii(0); ii < 6; ++ii)
{
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
state->m_jointForceTorque[ii] = status->m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[6 * jointIndex + ii];
}
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_jointMotorForce[jointIndex];
state->m_jointMotorTorque = status->m_sendActualStateArgs.m_stateDetails->m_jointMotorForce[jointIndex];
return 1;
}
}
@@ -1042,12 +1047,12 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
state->m_uDofSize = info.m_uSize;
for (int i = 0; i < state->m_qDofSize; i++)
{
state->m_jointPosition[i] = status->m_sendActualStateArgs.m_actualStateQ[info.m_qIndex + i];
state->m_jointPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQ[info.m_qIndex + i];
}
for (int i = 0; i < state->m_uDofSize; i++)
{
state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_actualStateQdot[info.m_uIndex+i];
state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_jointMotorForceMultiDof[info.m_uIndex + i];
state->m_jointVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[info.m_uIndex+i];
state->m_jointMotorTorqueMultiDof[i] = status->m_sendActualStateArgs.m_stateDetails->m_jointMotorForceMultiDof[info.m_uIndex + i];
}
}
else
@@ -1057,7 +1062,7 @@ B3_SHARED_API int b3GetJointStateMultiDof(b3PhysicsClientHandle physClient, b3Sh
}
for (int ii(0); ii < 6; ++ii)
{
state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_jointReactionForces[6 * jointIndex + ii];
state->m_jointReactionForceTorque[ii] = status->m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[6 * jointIndex + ii];
}
return 1;
@@ -1083,15 +1088,15 @@ B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemor
for (int i = 0; i < 3; ++i)
{
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6 * linkIndex + i];
state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_linkWorldVelocities[6 * linkIndex + i + 3];
state->m_worldPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkState[7 * linkIndex + i];
state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[7 * linkIndex + i];
state->m_worldLinearVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[6 * linkIndex + i];
state->m_worldAngularVelocity[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[6 * linkIndex + i + 3];
}
for (int i = 0; i < 4; ++i)
{
state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkState[7 * linkIndex + 3 + i];
state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
}
com.setOrigin(b3MakeVector3(state->m_worldPosition[0], state->m_worldPosition[1], state->m_worldPosition[2]));
com.setRotation(b3Quaternion(state->m_worldOrientation[0], state->m_worldOrientation[1], state->m_worldOrientation[2], state->m_worldOrientation[3]));
@@ -2338,19 +2343,19 @@ B3_SHARED_API int b3GetStatusActualState2(b3SharedMemoryStatusHandle statusHandl
}
if (linkLocalInertialFrames)
{
*linkLocalInertialFrames = args.m_linkLocalInertialFrames;
*linkLocalInertialFrames = args.m_stateDetails->m_linkLocalInertialFrames;
}
if (jointMotorForces)
{
*jointMotorForces = args.m_jointMotorForce;
*jointMotorForces = args.m_stateDetails->m_jointMotorForce;
}
if (linkStates)
{
*linkStates = args.m_linkState;
*linkStates = args.m_stateDetails->m_linkState;
}
if (linkWorldVelocities)
{
*linkWorldVelocities = args.m_linkWorldVelocities;
*linkWorldVelocities = args.m_stateDetails->m_linkWorldVelocities;
}
return 1;
}
@@ -2391,15 +2396,15 @@ B3_SHARED_API int b3GetStatusActualState(b3SharedMemoryStatusHandle statusHandle
}
if (actualStateQ)
{
*actualStateQ = args.m_actualStateQ;
*actualStateQ = args.m_stateDetails->m_actualStateQ;
}
if (actualStateQdot)
{
*actualStateQdot = args.m_actualStateQdot;
*actualStateQdot = args.m_stateDetails->m_actualStateQdot;
}
if (jointReactionForces)
{
*jointReactionForces = args.m_jointReactionForces;
*jointReactionForces = args.m_stateDetails->m_jointReactionForces;
}
return true;
}

22
examples/SharedMemory/PhysicsClientSharedMemory.cpp
View File

@@ -70,6 +70,8 @@ struct PhysicsClientSharedMemoryInternalData
SharedMemoryStatus m_lastServerStatus;
SendActualStateSharedMemoryStorage m_cachedState;
int m_counter;
bool m_isConnected;
@@ -79,6 +81,8 @@ struct PhysicsClientSharedMemoryInternalData
bool m_verboseOutput;
double m_timeOutInSeconds;
PhysicsClientSharedMemoryInternalData()
: m_sharedMemory(0),
m_ownsSharedMemory(false),
@@ -524,6 +528,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
if (serverCmd.m_type==CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
m_data->m_cachedState = *serverState;
//ideally we provided a 'getCachedState' but that would require changing the API, so we store a pointer instead.
m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState;
}
m_data->m_lastServerStatus = serverCmd;
// EnumSharedMemoryServerStatus s = (EnumSharedMemoryServerStatus)serverCmd.m_type;
@@ -827,6 +840,7 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
B3_PROFILE("CMD_ACTUAL_STATE_UPDATE_COMPLETED");
if (m_data->m_verboseOutput)
{
b3Printf("Received actual state\n");
@@ -845,12 +859,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
if (i < numQ - 1)
{
sprintf(msg, "%s%f,", msg,
command.m_sendActualStateArgs.m_actualStateQ[i]);
m_data->m_cachedState.m_actualStateQ[i]);
}
else
{
sprintf(msg, "%s%f", msg,
command.m_sendActualStateArgs.m_actualStateQ[i]);
m_data->m_cachedState.m_actualStateQ[i]);
}
}
sprintf(msg, "%s]", msg);
@@ -864,12 +878,12 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus()
if (i < numU - 1)
{
sprintf(msg, "%s%f,", msg,
command.m_sendActualStateArgs.m_actualStateQdot[i]);
m_data->m_cachedState.m_actualStateQdot[i]);
}
else
{
sprintf(msg, "%s%f", msg,
command.m_sendActualStateArgs.m_actualStateQdot[i]);
m_data->m_cachedState.m_actualStateQdot[i]);
}
}
sprintf(msg, "%s]", msg);

5
examples/SharedMemory/PhysicsDirect.cpp
View File

@@ -79,6 +79,8 @@ struct PhysicsDirectInternalData
bool m_ownsCommandProcessor;
double m_timeOutInSeconds;
SendActualStateSharedMemoryStorage m_cachedState;
PhysicsDirectInternalData()
: m_hasStatus(false),
m_verboseOutput(false),
@@ -1014,6 +1016,9 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd
}
case CMD_ACTUAL_STATE_UPDATE_COMPLETED:
{
SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)&m_data->m_bulletStreamDataServerToClient[0];
m_data->m_cachedState = *serverState;
m_data->m_serverStatus.m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState;
break;
}
case CMD_DESIRED_STATE_RECEIVED_COMPLETED:

3
examples/SharedMemory/PhysicsLoopBack.cpp
View File

@@ -173,6 +173,9 @@ void PhysicsLoopBack::getCachedCameraImage(struct b3CameraImageData* cameraData)
return m_data->m_physicsClient->getCachedCameraImage(cameraData);
}
void PhysicsLoopBack::getCachedContactPointInformation(struct b3ContactInformation* contactPointData)
{
return m_data->m_physicsClient->getCachedContactPointInformation(contactPointData);

136
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -6326,15 +6326,26 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
int bodyUniqueId = clientCmd.m_requestActualStateInformationCommandArgument.m_bodyUniqueId;
InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
//we store the state details in the shared memory block, to reduce status size
SendActualStateSharedMemoryStorage* stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
//make sure the storage fits, otherwise
btAssert(sizeof(SendActualStateSharedMemoryStorage) < bufferSizeInBytes);
if (sizeof(SendActualStateSharedMemoryStorage) > bufferSizeInBytes)
{
//this forces an error report
body = 0;
}
if (body && body->m_multiBody)
{
btMultiBody* mb = body->m_multiBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverStatusOut.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = body->m_multiBody->getNumLinks();
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
@@ -6370,26 +6381,26 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
body->m_rootLocalInertialFrame.getRotation()[3];
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ += 7; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = mb->getBaseVel()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = mb->getBaseVel()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = mb->getBaseVel()[2];
stateDetails->m_actualStateQdot[0] = mb->getBaseVel()[0];
stateDetails->m_actualStateQdot[1] = mb->getBaseVel()[1];
stateDetails->m_actualStateQdot[2] = mb->getBaseVel()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = mb->getBaseOmega()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = mb->getBaseOmega()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = mb->getBaseOmega()[2];
stateDetails->m_actualStateQdot[3] = mb->getBaseOmega()[0];
stateDetails->m_actualStateQdot[4] = mb->getBaseOmega()[1];
stateDetails->m_actualStateQdot[5] = mb->getBaseOmega()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
}
@@ -6421,11 +6432,11 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
{
for (int d = 0; d < mb->getLink(l).m_posVarCount; d++)
{
serverCmd.m_sendActualStateArgs.m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
stateDetails->m_actualStateQ[totalDegreeOfFreedomQ++] = mb->getJointPosMultiDof(l)[d];
}
for (int d = 0; d < mb->getLink(l).m_dofCount; d++)
{
serverCmd.m_sendActualStateArgs.m_jointMotorForce[totalDegreeOfFreedomU] = 0;
stateDetails->m_jointMotorForce[totalDegreeOfFreedomU] = 0;
if (mb->getLink(l).m_jointType == btMultibodyLink::eSpherical)
{
@@ -6434,7 +6445,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
{
btScalar impulse = motor->getAppliedImpulse(d);
btScalar force = impulse / m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
}
}
else
@@ -6446,19 +6457,19 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
if (motor && m_data->m_physicsDeltaTime > btScalar(0))
{
btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
stateDetails->m_jointMotorForceMultiDof[totalDegreeOfFreedomU] = force;
}
}
}
serverCmd.m_sendActualStateArgs.m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
stateDetails->m_actualStateQdot[totalDegreeOfFreedomU++] = mb->getJointVelMultiDof(l)[d];
}
if (0 == mb->getLink(l).m_jointFeedback)
{
for (int d = 0; d < 6; d++)
{
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + d] = 0;
stateDetails->m_jointReactionForces[l * 6 + d] = 0;
}
}
else
@@ -6466,16 +6477,16 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
btVector3 sensedForce = mb->getLink(l).m_jointFeedback->m_reactionForces.getLinear();
btVector3 sensedTorque = mb->getLink(l).m_jointFeedback->m_reactionForces.getAngular();
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 0] = sensedForce[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 1] = sensedForce[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 2] = sensedForce[2];
stateDetails->m_jointReactionForces[l * 6 + 0] = sensedForce[0];
stateDetails->m_jointReactionForces[l * 6 + 1] = sensedForce[1];
stateDetails->m_jointReactionForces[l * 6 + 2] = sensedForce[2];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 3] = sensedTorque[0];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 4] = sensedTorque[1];
serverCmd.m_sendActualStateArgs.m_jointReactionForces[l * 6 + 5] = sensedTorque[2];
stateDetails->m_jointReactionForces[l * 6 + 3] = sensedTorque[0];
stateDetails->m_jointReactionForces[l * 6 + 4] = sensedTorque[1];
stateDetails->m_jointReactionForces[l * 6 + 5] = sensedTorque[2];
}
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] = 0;
stateDetails->m_jointMotorForce[l] = 0;
if (supportsJointMotor(mb, l))
{
@@ -6484,7 +6495,7 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
if (motor && m_data->m_physicsDeltaTime > btScalar(0))
{
btScalar force = motor->getAppliedImpulse(0) / m_data->m_physicsDeltaTime;
serverCmd.m_sendActualStateArgs.m_jointMotorForce[l] =
stateDetails->m_jointMotorForce[l] =
force;
//if (force>0)
//{
@@ -6498,13 +6509,13 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
btVector3 linkCOMOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin();
btQuaternion linkCOMRotation = mb->getLink(l).m_cachedWorldTransform.getRotation();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 0] = linkCOMOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 1] = linkCOMOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 2] = linkCOMOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 3] = linkCOMRotation.x();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 4] = linkCOMRotation.y();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 5] = linkCOMRotation.z();
serverCmd.m_sendActualStateArgs.m_linkState[l * 7 + 6] = linkCOMRotation.w();
stateDetails->m_linkState[l * 7 + 0] = linkCOMOrigin.getX();
stateDetails->m_linkState[l * 7 + 1] = linkCOMOrigin.getY();
stateDetails->m_linkState[l * 7 + 2] = linkCOMOrigin.getZ();
stateDetails->m_linkState[l * 7 + 3] = linkCOMRotation.x();
stateDetails->m_linkState[l * 7 + 4] = linkCOMRotation.y();
stateDetails->m_linkState[l * 7 + 5] = linkCOMRotation.z();
stateDetails->m_linkState[l * 7 + 6] = linkCOMRotation.w();
btVector3 worldLinVel(0, 0, 0);
btVector3 worldAngVel(0, 0, 0);
@@ -6516,21 +6527,21 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
worldAngVel = linkRotMat * omega[l + 1];
}
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 0] = worldLinVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 1] = worldLinVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 2] = worldLinVel[2];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 3] = worldAngVel[0];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 4] = worldAngVel[1];
serverCmd.m_sendActualStateArgs.m_linkWorldVelocities[l * 6 + 5] = worldAngVel[2];
stateDetails->m_linkWorldVelocities[l * 6 + 0] = worldLinVel[0];
stateDetails->m_linkWorldVelocities[l * 6 + 1] = worldLinVel[1];
stateDetails->m_linkWorldVelocities[l * 6 + 2] = worldLinVel[2];
stateDetails->m_linkWorldVelocities[l * 6 + 3] = worldAngVel[0];
stateDetails->m_linkWorldVelocities[l * 6 + 4] = worldAngVel[1];
stateDetails->m_linkWorldVelocities[l * 6 + 5] = worldAngVel[2];
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 0] = linkLocalInertialOrigin.getX();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 1] = linkLocalInertialOrigin.getY();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 2] = linkLocalInertialOrigin.getZ();
stateDetails->m_linkLocalInertialFrames[l * 7 + 0] = linkLocalInertialOrigin.getX();
stateDetails->m_linkLocalInertialFrames[l * 7 + 1] = linkLocalInertialOrigin.getY();
stateDetails->m_linkLocalInertialFrames[l * 7 + 2] = linkLocalInertialOrigin.getZ();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 3] = linkLocalInertialRotation.x();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 4] = linkLocalInertialRotation.y();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 5] = linkLocalInertialRotation.z();
serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l * 7 + 6] = linkLocalInertialRotation.w();
stateDetails->m_linkLocalInertialFrames[l * 7 + 3] = linkLocalInertialRotation.x();
stateDetails->m_linkLocalInertialFrames[l * 7 + 4] = linkLocalInertialRotation.y();
stateDetails->m_linkLocalInertialFrames[l * 7 + 5] = linkLocalInertialRotation.z();
stateDetails->m_linkLocalInertialFrames[l * 7 + 6] = linkLocalInertialRotation.w();
}
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;
@@ -6545,35 +6556,36 @@ bool PhysicsServerCommandProcessor::processRequestActualStateCommand(const struc
btRigidBody* rb = body->m_rigidBody;
SharedMemoryStatus& serverCmd = serverStatusOut;
serverCmd.m_type = CMD_ACTUAL_STATE_UPDATE_COMPLETED;
serverCmd.m_sendActualStateArgs.m_bodyUniqueId = bodyUniqueId;
serverCmd.m_sendActualStateArgs.m_numLinks = 0;
serverCmd.m_sendActualStateArgs.m_stateDetails = 0;
int totalDegreeOfFreedomQ = 0;
int totalDegreeOfFreedomU = 0;
btTransform tr = rb->getWorldTransform();
//base position in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[0] = tr.getOrigin()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[1] = tr.getOrigin()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[2] = tr.getOrigin()[2];
stateDetails->m_actualStateQ[0] = tr.getOrigin()[0];
stateDetails->m_actualStateQ[1] = tr.getOrigin()[1];
stateDetails->m_actualStateQ[2] = tr.getOrigin()[2];
//base orientation, quaternion x,y,z,w, in world space, carthesian
serverCmd.m_sendActualStateArgs.m_actualStateQ[3] = tr.getRotation()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQ[4] = tr.getRotation()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQ[5] = tr.getRotation()[2];
serverCmd.m_sendActualStateArgs.m_actualStateQ[6] = tr.getRotation()[3];
stateDetails->m_actualStateQ[3] = tr.getRotation()[0];
stateDetails->m_actualStateQ[4] = tr.getRotation()[1];
stateDetails->m_actualStateQ[5] = tr.getRotation()[2];
stateDetails->m_actualStateQ[6] = tr.getRotation()[3];
totalDegreeOfFreedomQ += 7; //pos + quaternion
//base linear velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[0] = rb->getLinearVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[1] = rb->getLinearVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[2] = rb->getLinearVelocity()[2];
stateDetails->m_actualStateQdot[0] = rb->getLinearVelocity()[0];
stateDetails->m_actualStateQdot[1] = rb->getLinearVelocity()[1];
stateDetails->m_actualStateQdot[2] = rb->getLinearVelocity()[2];
//base angular velocity (in world space, carthesian)
serverCmd.m_sendActualStateArgs.m_actualStateQdot[3] = rb->getAngularVelocity()[0];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[4] = rb->getAngularVelocity()[1];
serverCmd.m_sendActualStateArgs.m_actualStateQdot[5] = rb->getAngularVelocity()[2];
stateDetails->m_actualStateQdot[3] = rb->getAngularVelocity()[0];
stateDetails->m_actualStateQdot[4] = rb->getAngularVelocity()[1];
stateDetails->m_actualStateQdot[5] = rb->getAngularVelocity()[2];
totalDegreeOfFreedomU += 6; //3 linear and 3 angular DOF
serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ;

11
examples/SharedMemory/SharedMemoryCommandProcessor.cpp
View File

@@ -17,7 +17,8 @@ struct SharedMemoryCommandProcessorInternalData
bool m_waitingForServer;
SharedMemoryStatus m_lastServerStatus;
SharedMemoryBlock* m_testBlock1;
SendActualStateSharedMemoryStorage m_cachedState;
SharedMemoryCommandProcessorInternalData()
: m_sharedMemoryKey(SHARED_MEMORY_KEY),
m_isConnected(false),
@@ -160,6 +161,14 @@ bool SharedMemoryCommandProcessor::receiveStatus(struct SharedMemoryStatus& serv
m_data->m_testBlock1->m_numProcessedServerCommands + 1);
const SharedMemoryStatus& serverCmd = m_data->m_testBlock1->m_serverCommands[0];
if (serverCmd.m_type == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
{
SendActualStateSharedMemoryStorage* serverState = (SendActualStateSharedMemoryStorage*)m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;
m_data->m_cachedState = *serverState;
//ideally we provided a 'getCachedState' but that would require changing the API, so we store a pointer instead.
m_data->m_testBlock1->m_serverCommands[0].m_sendActualStateArgs.m_stateDetails = &m_data->m_cachedState;
}
m_data->m_lastServerStatus = serverCmd;
m_data->m_lastServerStatus.m_dataStream = m_data->m_testBlock1->m_bulletStreamDataServerToClientRefactor;

5
examples/SharedMemory/SharedMemoryCommands.h
View File

@@ -522,7 +522,12 @@ struct SendActualStateArgs
int m_numDegreeOfFreedomU;
double m_rootLocalInertialFrame[7];
struct SendActualStateSharedMemoryStorage* m_stateDetails;
};
struct SendActualStateSharedMemoryStorage
{
//actual state is only written by the server, read-only access by client is expected
double m_actualStateQ[MAX_DEGREE_OF_FREEDOM];
double m_actualStateQdot[MAX_DEGREE_OF_FREEDOM];

16
examples/SharedMemory/grpc/ConvertGRPCBullet.cpp
View File

@@ -1260,13 +1260,15 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
serverStatus.m_sendActualStateArgs.m_numDegreeOfFreedomQ = numDegreeOfFreedomQ;
serverStatus.m_sendActualStateArgs.m_numDegreeOfFreedomU = numDegreeOfFreedomU;
serverStatus.m_sendActualStateArgs.m_stateDetails = (SendActualStateSharedMemoryStorage*)bufferServerToClient;
for (int i = 0; i < numDegreeOfFreedomQ; i++)
{
serverStatus.m_sendActualStateArgs.m_actualStateQ[i] = stat->actualstateq(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_actualStateQ[i] = stat->actualstateq(i);
}
for (int i = 0; i < numDegreeOfFreedomU; i++)
{
serverStatus.m_sendActualStateArgs.m_actualStateQdot[i] = stat->actualstateqdot(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_actualStateQdot[i] = stat->actualstateqdot(i);
}
for (int i = 0; i < 7; i++)
{
@@ -1274,23 +1276,23 @@ bool convertGRPCToStatus(const PyBulletStatus& grpcReply, SharedMemoryStatus& se
}
for (int i = 0; i < numLinks * 6; i++)
{
serverStatus.m_sendActualStateArgs.m_linkLocalInertialFrames[i] = stat->linklocalinertialframes(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkLocalInertialFrames[i] = stat->linklocalinertialframes(i);
}
for (int i = 0; i < numLinks * 6; i++)
{
serverStatus.m_sendActualStateArgs.m_jointReactionForces[i] = stat->jointreactionforces(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_jointReactionForces[i] = stat->jointreactionforces(i);
}
for (int i = 0; i < numLinks; i++)
{
serverStatus.m_sendActualStateArgs.m_jointMotorForce[i] = stat->jointmotorforce(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_jointMotorForce[i] = stat->jointmotorforce(i);
}
for (int i = 0; i < numLinks * 7; i++)
{
serverStatus.m_sendActualStateArgs.m_linkState[i] = stat->linkstate(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkState[i] = stat->linkstate(i);
}
for (int i = 0; i < numLinks * 6; i++)
{
serverStatus.m_sendActualStateArgs.m_linkWorldVelocities[i] = stat->linkworldvelocities(i);
serverStatus.m_sendActualStateArgs.m_stateDetails->m_linkWorldVelocities[i] = stat->linkworldvelocities(i);
}
break;

1
src/BulletDynamics/ConstraintSolver/btConstraintSolver.h
View File

@@ -35,6 +35,7 @@ enum btConstraintSolverType
BT_MLCP_SOLVER = 2,
BT_NNCG_SOLVER = 4,
BT_MULTIBODY_SOLVER = 8,
BT_BLOCK_SOLVER = 16,
};
class btConstraintSolver

871
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.cpp
View File

File diff suppressed because it is too large Load Diff

144
src/BulletDynamics/ConstraintSolver/btSequentialImpulseConstraintSolver.h
View File

@@ -29,6 +29,68 @@ class btCollisionObject;
typedef btScalar (*btSingleConstraintRowSolver)(btSolverBody&, btSolverBody&, const btSolverConstraint&);
struct btSISolverSingleIterationData
{
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_seed;
btSingleConstraintRowSolver& m_resolveSingleConstraintRowGeneric;
btSingleConstraintRowSolver& m_resolveSingleConstraintRowLowerLimit;
btSingleConstraintRowSolver& m_resolveSplitPenetrationImpulse;
btAlignedObjectArray<int>& m_kinematicBodyUniqueIdToSolverBodyTable;
int& m_fixedBodyId;
int& m_maxOverrideNumSolverIterations;
int getOrInitSolverBody(btCollisionObject & body, btScalar timeStep);
static void initSolverBody(btSolverBody * solverBody, btCollisionObject * collisionObject, btScalar timeStep);
int getSolverBody(btCollisionObject& body) const;
btSISolverSingleIterationData(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
btConstraintArray& tmpSolverContactConstraintPool,
btConstraintArray& tmpSolverNonContactConstraintPool,
btConstraintArray& tmpSolverContactFrictionConstraintPool,
btConstraintArray& tmpSolverContactRollingFrictionConstraintPool,
btAlignedObjectArray<int>& orderTmpConstraintPool,
btAlignedObjectArray<int>& orderNonContactConstraintPool,
btAlignedObjectArray<int>& orderFrictionConstraintPool,
btAlignedObjectArray<btTypedConstraint::btConstraintInfo1>& tmpConstraintSizesPool,
btSingleConstraintRowSolver& resolveSingleConstraintRowGeneric,
btSingleConstraintRowSolver& resolveSingleConstraintRowLowerLimit,
btSingleConstraintRowSolver& resolveSplitPenetrationImpulse,
btAlignedObjectArray<int>& kinematicBodyUniqueIdToSolverBodyTable,
unsigned long& seed,
int& fixedBodyId,
int& maxOverrideNumSolverIterations
)
:m_tmpSolverBodyPool(tmpSolverBodyPool),
m_tmpSolverContactConstraintPool(tmpSolverContactConstraintPool),
m_tmpSolverNonContactConstraintPool(tmpSolverNonContactConstraintPool),
m_tmpSolverContactFrictionConstraintPool(tmpSolverContactFrictionConstraintPool),
m_tmpSolverContactRollingFrictionConstraintPool(tmpSolverContactRollingFrictionConstraintPool),
m_orderTmpConstraintPool(orderTmpConstraintPool),
m_orderNonContactConstraintPool(orderNonContactConstraintPool),
m_orderFrictionConstraintPool(orderFrictionConstraintPool),
m_tmpConstraintSizesPool(tmpConstraintSizesPool),
m_resolveSingleConstraintRowGeneric(resolveSingleConstraintRowGeneric),
m_resolveSingleConstraintRowLowerLimit(resolveSingleConstraintRowLowerLimit),
m_resolveSplitPenetrationImpulse(resolveSplitPenetrationImpulse),
m_kinematicBodyUniqueIdToSolverBodyTable(kinematicBodyUniqueIdToSolverBodyTable),
m_seed(seed),
m_fixedBodyId(fixedBodyId),
m_maxOverrideNumSolverIterations(maxOverrideNumSolverIterations)
{
}
};
///The btSequentialImpulseConstraintSolver is a fast SIMD implementation of the Projected Gauss Seidel (iterative LCP) method.
ATTRIBUTE_ALIGNED16(class)
btSequentialImpulseConstraintSolver : public btConstraintSolver
@@ -64,26 +126,26 @@ protected:
btScalar m_leastSquaresResidual;
void setupFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
const btContactSolverInfo& infoGlobal,
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
void setupTorsionalFrictionConstraint(btSolverConstraint & solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
btSolverConstraint& addFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
btSolverConstraint& addTorsionalFrictionConstraint(const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar torsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.f);
void setupContactConstraint(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp,
const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
const btContactSolverInfo& infoGlobal, btScalar& relaxation, const btVector3& rel_pos1, const btVector3& rel_pos2);
static void applyAnisotropicFriction(btCollisionObject * colObj, btVector3 & frictionDirection, int frictionMode);
void setFrictionConstraintImpulse(btSolverConstraint & solverConstraint, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
///m_btSeed2 is used for re-arranging the constraint rows. improves convergence/quality of friction
unsigned long m_btSeed2;
@@ -97,6 +159,7 @@ protected:
virtual void convertJoints(btTypedConstraint * *constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
void convertJoint(btSolverConstraint * currentConstraintRow, btTypedConstraint * constraint, const btTypedConstraint::btConstraintInfo1& info1, int solverBodyIdA, int solverBodyIdB, const btContactSolverInfo& infoGlobal);
virtual void convertBodies(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
btScalar resolveSplitPenetrationSIMD(btSolverBody & bodyA, btSolverBody & bodyB, const btSolverConstraint& contactConstraint)
@@ -122,7 +185,8 @@ protected:
return m_resolveSplitPenetrationImpulse(bodyA, bodyB, contactConstraint);
}
protected:
public:
void writeBackContacts(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackJoints(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
void writeBackBodies(int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
@@ -130,6 +194,7 @@ protected:
virtual btScalar solveGroupCacheFriendlyFinish(btCollisionObject * *bodies, int numBodies, const btContactSolverInfo& infoGlobal);
virtual btScalar solveSingleIteration(int iteration, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlySetup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
virtual btScalar solveGroupCacheFriendlyIterations(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
@@ -141,13 +206,52 @@ public:
virtual btScalar solveGroup(btCollisionObject * *bodies, int numBodies, btPersistentManifold** manifold, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& info, btIDebugDraw* debugDrawer, btDispatcher* dispatcher);
static btScalar solveSingleIterationInternal(btSISolverSingleIterationData& siData, int iteration, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
static void convertBodiesInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
static void convertJointsInternal(btSISolverSingleIterationData& siData, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal);
static void convertContactInternal(btSISolverSingleIterationData& siData, btPersistentManifold * manifold, const btContactSolverInfo& infoGlobal);
static void setupContactConstraintInternal(btSISolverSingleIterationData& siData, btSolverConstraint& solverConstraint, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btContactSolverInfo& infoGlobal, btScalar& relaxation,
const btVector3& rel_pos1, const btVector3& rel_pos2);
static btScalar restitutionCurveInternal(btScalar rel_vel, btScalar restitution, btScalar velocityThreshold);
static btSolverConstraint& addTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactRollingFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, btScalar desiredVelocity = 0, btScalar cfmSlip = 0.);
static void setupTorsionalFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis1, int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, btScalar combinedTorsionalFriction, const btVector3& rel_pos1, const btVector3& rel_pos2,
btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation,
btScalar desiredVelocity, btScalar cfmSlip);
static void setupFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btSolverConstraint& solverConstraint, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity, btScalar cfmSlip);
static btSolverConstraint& addFrictionConstraintInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, const btVector3& normalAxis, int solverBodyIdA, int solverBodyIdB, int frictionIndex, btManifoldPoint& cp, const btVector3& rel_pos1, const btVector3& rel_pos2, btCollisionObject* colObj0, btCollisionObject* colObj1, btScalar relaxation, const btContactSolverInfo& infoGlobal, btScalar desiredVelocity = 0., btScalar cfmSlip = 0.);
static void setFrictionConstraintImpulseInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, btConstraintArray& tmpSolverContactFrictionConstraintPool,
btSolverConstraint& solverConstraint,
int solverBodyIdA, int solverBodyIdB,
btManifoldPoint& cp, const btContactSolverInfo& infoGlobal);
static void convertJointInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool,
int& maxOverrideNumSolverIterations,
btSolverConstraint* currentConstraintRow,
btTypedConstraint* constraint,
const btTypedConstraint::btConstraintInfo1& info1,
int solverBodyIdA,
int solverBodyIdB,
const btContactSolverInfo& infoGlobal);
static btScalar solveGroupCacheFriendlyFinishInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, const btContactSolverInfo& infoGlobal);
static void writeBackContactsInternal(btConstraintArray& tmpSolverContactConstraintPool, btConstraintArray& tmpSolverContactFrictionConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void writeBackJointsInternal(btConstraintArray& tmpSolverNonContactConstraintPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void writeBackBodiesInternal(btAlignedObjectArray<btSolverBody>& tmpSolverBodyPool, int iBegin, int iEnd, const btContactSolverInfo& infoGlobal);
static void solveGroupCacheFriendlySplitImpulseIterationsInternal(btSISolverSingleIterationData& siData, btCollisionObject** bodies, int numBodies, btPersistentManifold** manifoldPtr, int numManifolds, btTypedConstraint** constraints, int numConstraints, const btContactSolverInfo& infoGlobal, btIDebugDraw* debugDrawer);
///clear internal cached data and reset random seed
virtual void reset();
unsigned long btRand2();
int btRandInt2(int n);
static unsigned long btRand2a(unsigned long& seed);
static int btRandInt2a(int n, unsigned long& seed);
void setRandSeed(unsigned long seed)
{
m_btSeed2 = seed;
@@ -180,14 +284,18 @@ public:
}
///Various implementations of solving a single constraint row using a generic equality constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getScalarConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverGeneric();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverGeneric();
///Various implementations of solving a single constraint row using an inequality (lower limit) constraint, using scalar reference, SSE2 or SSE4
btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getScalarConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE2ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getSSE4_1ConstraintRowSolverLowerLimit();
static btSingleConstraintRowSolver getScalarSplitPenetrationImpulseGeneric();
static btSingleConstraintRowSolver getSSE2SplitPenetrationImpulseGeneric();
};
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H
#endif //BT_SEQUENTIAL_IMPULSE_CONSTRAINT_SOLVER_H

2
src/BulletDynamics/Featherstone/btMultiBodyMLCPConstraintSolver.h
View File

@@ -156,7 +156,7 @@ protected:
btTypedConstraint** constraints,
int numConstraints,
const btContactSolverInfo& infoGlobal,
btIDebugDraw* debugDrawer) BT_OVERRIDE;
btIDebugDraw* debugDrawer) ;
public:
BT_DECLARE_ALIGNED_ALLOCATOR()