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

fix load_soft_body.py example.

Browse Source 
add optional flags in pybullet.resetSimulation.
fix compile issue due to SKIP_DEFORMABLE_WORLD
fix issue in .obj importer (todo: replace with tiny_obj_loader)
todo: replace std::ifstream fs; by fileIO usage.
This commit is contained in:
Erwin Coumans
2019-11-14 21:20:42 -08:00
committed by Xuchen Han
parent dc26d2e360
commit 655981c6ad
8 changed files with 349 additions and 151 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
examples/SharedMemory/PhysicsClientC_API.cpp
View File

@@ -923,6 +923,19 @@ B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3Shared
return (b3SharedMemoryCommandHandle)command;
}
B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags)
{
struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle;
b3Assert(command);
btAssert(command->m_type == CMD_RESET_SIMULATION);
if (command->m_type == CMD_RESET_SIMULATION)
{
command->m_updateFlags = flags;
}
return 0;
}
B3_SHARED_API b3SharedMemoryCommandHandle b3JointControlCommandInit(b3PhysicsClientHandle physClient, int controlMode)
{
return b3JointControlCommandInit2(physClient, 0, controlMode);

1
examples/SharedMemory/PhysicsClientC_API.h
View File

@@ -373,6 +373,7 @@ extern "C"
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand(b3PhysicsClientHandle physClient);
B3_SHARED_API b3SharedMemoryCommandHandle b3InitResetSimulationCommand2(b3SharedMemoryCommandHandle commandHandle);
B3_SHARED_API int b3InitResetSimulationSetFlags(b3SharedMemoryCommandHandle commandHandle, int flags);
///Load a robot from a URDF file. Status type will CMD_URDF_LOADING_COMPLETED.
///Access the robot from the unique body index, through b3GetStatusBodyIndex(statusHandle);

387
examples/SharedMemory/PhysicsServerCommandProcessor.cpp
View File

@@ -92,6 +92,11 @@
#include "../TinyAudio/b3SoundEngine.h"
#endif
#ifdef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#define SKIP_DEFORMABLE_BODY 1
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#include "BulletSoftBody/btSoftBodyRigidBodyCollisionConfiguration.h"
#include "BulletSoftBody/btSoftBodySolvers.h"
@@ -101,11 +106,17 @@
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#include "../SoftDemo/BunnyMesh.h"
#else
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#endif
#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
#include "BulletSoftBody/btDeformableMultiBodyDynamicsWorld.h"
#include "BulletSoftBody/btDeformableBodySolver.h"
#include "BulletSoftBody/btDeformableMultiBodyConstraintSolver.h"
#endif//SKIP_DEFORMABLE_BODY
#include "BulletDynamics/Featherstone/btMultiBodyDynamicsWorld.h"
#define SKIP_DEFORMABLE_BODY 1
int gInternalSimFlags = 0;
bool gResetSimulation = 0;
@@ -1619,26 +1630,24 @@ struct PhysicsServerCommandProcessorInternalData
btHashedOverlappingPairCache* m_pairCache;
btBroadphaseInterface* m_broadphase;
btCollisionDispatcher* m_dispatcher;
#if defined(SKIP_DEFORMABLE_BODY) || defined(SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD)
btMultiBodyConstraintSolver* m_solver;
#else
btDeformableMultiBodyConstraintSolver* m_solver;
#endif
btDefaultCollisionConfiguration* m_collisionConfiguration;
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* m_dynamicsWorld;
//#ifndef SKIP_DEFORMABLE_BODY
btDeformableBodySolver* m_deformablebodySolver;
btAlignedObjectArray<btDeformableLagrangianForce*> m_lf;
#else
btSoftMultiBodyDynamicsWorld* m_dynamicsWorld;
btSoftBodySolver* m_softbodySolver;
#endif
#else
//#else//SKIP_DEFORMABLE_BODY
//#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
//#else//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btMultiBodyDynamicsWorld* m_dynamicsWorld;
#endif
//#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
//#endif//SKIP_DEFORMABLE_BODY
int m_constraintSolverType;
SharedMemoryDebugDrawer* m_remoteDebugDrawer;
@@ -1705,6 +1714,7 @@ struct PhysicsServerCommandProcessorInternalData
m_solver(0),
m_collisionConfiguration(0),
m_dynamicsWorld(0),
m_deformablebodySolver(0),
m_constraintSolverType(-1),
m_remoteDebugDrawer(0),
m_stateLoggersUniqueId(0),
@@ -2592,7 +2602,27 @@ struct ProgrammaticUrdfInterface : public URDFImporterInterface
}
};
void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
btDeformableMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getDeformableWorld()
{
btDeformableMultiBodyDynamicsWorld* world = 0;
if (m_data->m_dynamicsWorld->getWorldType()== BT_DEFORMABLE_MULTIBODY_DYNAMICS_WORLD)
{
world = (btDeformableMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
}
return world;
}
btSoftMultiBodyDynamicsWorld* PhysicsServerCommandProcessor::getSoftWorld()
{
btSoftMultiBodyDynamicsWorld* world = 0;
if (m_data->m_dynamicsWorld->getWorldType()== BT_SOFT_MULTIBODY_DYNAMICS_WORLD)
{
world = (btSoftMultiBodyDynamicsWorld*) m_data->m_dynamicsWorld;
}
return world;
}
void PhysicsServerCommandProcessor::createEmptyDynamicsWorld(int flags)
{
m_data->m_constraintSolverType = eConstraintSolverLCP_SI;
///collision configuration contains default setup for memory, collision setup
@@ -2613,27 +2643,40 @@ void PhysicsServerCommandProcessor::createEmptyDynamicsWorld()
m_data->m_pairCache->setOverlapFilterCallback(m_data->m_broadphaseCollisionFilterCallback);
//int maxProxies = 32768;
//m_data->m_broadphase = new btSimpleBroadphase(maxProxies, m_data->m_pairCache);
btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
if (flags&RESET_USE_SIMPLE_BROADPHASE)
{
m_data->m_broadphase = new btSimpleBroadphase(65536, m_data->m_pairCache);
} else
{
btDbvtBroadphase* bv = new btDbvtBroadphase(m_data->m_pairCache);
bv->setVelocityPrediction(0);
m_data->m_broadphase = bv;
}
#ifdef SKIP_DEFORMABLE_BODY
m_data->m_solver = new btMultiBodyConstraintSolver;
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (flags & RESET_USE_DEFORMABLE_WORLD)
{
#ifndef SKIP_DEFORMABLE_BODY
m_data->m_deformablebodySolver = new btDeformableBodySolver();
m_data->m_solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
#else
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
m_data->m_deformablebodySolver = new btDeformableBodySolver();
btDeformableMultiBodyConstraintSolver* solver = new btDeformableMultiBodyConstraintSolver;
m_data->m_solver = solver;
solver->setDeformableSolver(m_data->m_deformablebodySolver);
m_data->m_dynamicsWorld = new btDeformableMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, solver, m_data->m_collisionConfiguration, m_data->m_deformablebodySolver);
#endif
#else
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
}
if ((0==m_data->m_dynamicsWorld) && (0==(flags&RESET_USE_DISCRETE_DYNAMICS_WORLD)))
{
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btSoftMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
#endif
}
if (0==m_data->m_dynamicsWorld)
{
m_data->m_solver = new btMultiBodyConstraintSolver;
m_data->m_dynamicsWorld = new btMultiBodyDynamicsWorld(m_data->m_dispatcher, m_data->m_broadphase, m_data->m_solver, m_data->m_collisionConfiguration);
}
//Workaround: in a VR application, where we avoid synchronizing between GFX/Physics threads, we don't want to resize this array, so pre-allocate it
m_data->m_dynamicsWorld->getCollisionObjectArray().reserve(128 * 1024);
@@ -2800,13 +2843,35 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
m_data->m_lf.clear();
#endif
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for (i = m_data->m_dynamicsWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{
btSoftBody* sb = m_data->m_dynamicsWorld->getSoftBodyArray()[i];
m_data->m_dynamicsWorld->removeSoftBody(sb);
delete sb;
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
for (i =softWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{
btSoftBody* sb =softWorld->getSoftBodyArray()[i];
softWorld->removeSoftBody(sb);
delete sb;
}
}
}
#endif//SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
#ifndef SKIP_DEFORMABLE_BODY
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
for (i =deformWorld->getSoftBodyArray().size() - 1; i >= 0; i--)
{
btSoftBody* sb =deformWorld->getSoftBodyArray()[i];
deformWorld->removeSoftBody(sb);
delete sb;
}
}
}
#endif
}
for (int i = 0; i < constraints.size(); i++)
@@ -8019,11 +8084,6 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
b3FileUtils::extractPath(relativeFileName, pathPrefix, 1024);
}
// add _sim.vtk postfix
char relativeSimFileName[1024];
char sim_file_ending[9] = "_sim.vtk";
strncpy(relativeSimFileName, relativeFileName, strlen(relativeFileName));
strncpy(relativeSimFileName + strlen(relativeFileName)-4, sim_file_ending, sizeof(sim_file_ending));
const std::string& error_message_prefix = "";
std::string out_found_filename;
@@ -8035,7 +8095,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_filename, &out_type);
if (out_type == UrdfGeometry::FILE_OBJ)
{
bool foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeSimFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
foundFile = UrdfFindMeshFile(fileIO, pathPrefix, relativeFileName, error_message_prefix, &out_found_sim_filename, &out_sim_type);
render_mesh_is_sim_mesh = !foundFile;
}
@@ -8066,59 +8126,83 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
int numTris = shape.mesh.indices.size() / 3;
if (numTris > 0)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(m_data->m_dynamicsWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(softWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
}
}
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
psb = btSoftBodyHelpers::CreateFromTriMesh(deformWorld->getWorldInfo(), &vertices[0], &indices[0], numTris);
}
}
}
}
#ifndef SKIP_DEFORMABLE_BODY
btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false);
m_data->m_dynamicsWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, false);
deformWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
}
}
#endif
}
else if (out_sim_type == UrdfGeometry::FILE_VTK)
{
#ifndef SKIP_DEFORMABLE_BODY
psb = btSoftBodyHelpers::CreateFromVtkFile(m_data->m_dynamicsWorld->getWorldInfo(), out_found_sim_filename.c_str());
btScalar corotated_mu, corotated_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
{
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
m_data->m_dynamicsWorld->addForce(psb, corotatedForce);
m_data->m_lf.push_back(corotatedForce);
}
btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
{
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping;
btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
m_data->m_dynamicsWorld->addForce(psb, neohookeanForce);
m_data->m_lf.push_back(neohookeanForce);
}
btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true);
m_data->m_dynamicsWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
}
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
psb = btSoftBodyHelpers::CreateFromVtkFile(deformWorld->getWorldInfo(), out_found_sim_filename.c_str());
btScalar corotated_mu, corotated_lambda;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_COROTATED_FORCE)
{
corotated_mu = clientCmd.m_loadSoftBodyArguments.m_corotatedMu;
corotated_lambda = clientCmd.m_loadSoftBodyArguments.m_corotatedLambda;
btDeformableLagrangianForce* corotatedForce = new btDeformableCorotatedForce(corotated_mu, corotated_lambda);
deformWorld->addForce(psb, corotatedForce);
m_data->m_lf.push_back(corotatedForce);
}
btScalar neohookean_mu, neohookean_lambda, neohookean_damping;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE)
{
neohookean_mu = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanMu;
neohookean_lambda = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanLambda;
neohookean_damping = clientCmd.m_loadSoftBodyArguments.m_NeoHookeanDamping;
btDeformableLagrangianForce* neohookeanForce = new btDeformableNeoHookeanForce(neohookean_mu, neohookean_lambda, neohookean_damping);
deformWorld->addForce(psb, neohookeanForce);
m_data->m_lf.push_back(neohookeanForce);
}
btScalar spring_elastic_stiffness, spring_damping_stiffness;
if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_ADD_MASS_SPRING_FORCE)
{
spring_elastic_stiffness = clientCmd.m_loadSoftBodyArguments.m_springElasticStiffness;
spring_damping_stiffness = clientCmd.m_loadSoftBodyArguments.m_springDampingStiffness;
btDeformableLagrangianForce* springForce = new btDeformableMassSpringForce(spring_elastic_stiffness, spring_damping_stiffness, true);
deformWorld->addForce(psb, springForce);
m_data->m_lf.push_back(springForce);
}
}
#endif
}
if (psb != NULL)
{
#ifndef SKIP_DEFORMABLE_BODY
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
if (!render_mesh_is_sim_mesh)
{
// load render mesh
@@ -8133,7 +8217,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
psb->setCollisionQuadrature(5);
btVector3 gravity = m_data->m_dynamicsWorld->getGravity();
btDeformableLagrangianForce* gravityForce = new btDeformableGravityForce(gravity);
m_data->m_dynamicsWorld->addForce(psb, gravityForce);
deformWorld->addForce(psb, gravityForce);
m_data->m_lf.push_back(gravityForce);
btScalar collision_hardness = 1;
psb->m_cfg.kKHR = collision_hardness;
@@ -8171,25 +8255,44 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar
use_self_collision = loadSoftBodyArgs.m_useSelfCollision;
}
psb->setSelfCollision(use_self_collision);
#else
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_kLST = 0.5;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2, pm);
psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5;
//turn on softbody vs softbody collision
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
psb->randomizeConstraints();
psb->setTotalMass(mass, true);
#endif
}
#endif//SKIP_DEFORMABLE_BODY
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
btSoftBody::Material* pm = psb->appendMaterial();
pm->m_kLST = 0.5;
pm->m_flags -= btSoftBody::fMaterial::DebugDraw;
psb->generateBendingConstraints(2, pm);
psb->m_cfg.piterations = 20;
psb->m_cfg.kDF = 0.5;
//turn on softbody vs softbody collision
psb->m_cfg.collisions |= btSoftBody::fCollision::VF_SS;
psb->randomizeConstraints();
psb->setTotalMass(mass, true);
}
#endif //SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
psb->scale(btVector3(scale, scale, scale));
psb->rotate(initialOrn);
psb->translate(initialPos);
psb->getCollisionShape()->setMargin(collisionMargin);
psb->getCollisionShape()->setUserPointer(psb);
m_data->m_dynamicsWorld->addSoftBody(psb);
#ifndef SKIP_DEFORMABLE_BODY
if (deformWorld)
{
deformWorld->addSoftBody(psb);
} else
#endif//SKIP_DEFORMABLE_BODY
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->addSoftBody(psb);
}
}
m_data->m_guiHelper->createCollisionShapeGraphicsObject(psb->getCollisionShape());
m_data->m_guiHelper->autogenerateGraphicsObjects(this->m_data->m_dynamicsWorld);
int bodyUniqueId = m_data->m_bodyHandles.allocHandle();
@@ -9190,7 +9293,17 @@ bool PhysicsServerCommandProcessor::processSendPhysicsParametersCommand(const st
clientCmd.m_physSimParamArgs.m_gravityAcceleration[2]);
this->m_data->m_dynamicsWorld->setGravity(grav);
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
m_data->m_dynamicsWorld->getWorldInfo().m_gravity = grav;
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->getWorldInfo().m_gravity = grav;
}
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
deformWorld->getWorldInfo().m_gravity = grav;
}
#endif
if (m_data->m_verboseOutput)
@@ -9549,7 +9662,8 @@ bool PhysicsServerCommandProcessor::processResetSimulationCommand(const struct S
bool hasStatus = true;
BT_PROFILE("CMD_RESET_SIMULATION");
m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 0);
resetSimulation();
resetSimulation(clientCmd.m_updateFlags);
m_data->m_guiHelper->setVisualizerFlag(COV_ENABLE_SYNC_RENDERING_INTERNAL, 1);
SharedMemoryStatus& serverCmd = serverStatusOut;
@@ -10416,7 +10530,17 @@ bool PhysicsServerCommandProcessor::processRemoveBodyCommand(const struct Shared
m_data->m_pluginManager.getRenderInterface()->removeVisualShape(psb->getBroadphaseHandle()->getUid());
}
serverCmd.m_removeObjectArgs.m_bodyUniqueIds[serverCmd.m_removeObjectArgs.m_numBodies++] = bodyUniqueId;
m_data->m_dynamicsWorld->removeSoftBody(psb);
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->removeSoftBody(psb);
}
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
deformWorld->removeSoftBody(psb);
}
int graphicsInstance = psb->getUserIndex2();
m_data->m_guiHelper->removeGraphicsInstance(graphicsInstance);
delete psb;
@@ -12846,18 +12970,22 @@ void PhysicsServerCommandProcessor::renderScene(int renderFlags)
#ifndef SKIP_DEFORMABLE_BODY
if (m_data->m_dynamicsWorld)
{
if (m_data->m_dynamicsWorld->getSoftBodyArray().size())
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++)
if (deformWorld->getSoftBodyArray().size())
{
btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
{
btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags());
btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
{
btSoftBodyHelpers::DrawFrame(psb, m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), deformWorld->getDrawFlags());
}
}
m_data->m_guiHelper->drawDebugDrawerLines();
m_data->m_guiHelper->clearLines();
}
m_data->m_guiHelper->drawDebugDrawerLines();
m_data->m_guiHelper->clearLines();
}
}
#endif
@@ -12875,15 +13003,36 @@ void PhysicsServerCommandProcessor::physicsDebugDraw(int debugDrawFlags)
m_data->m_dynamicsWorld->debugDrawWorld();
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
for (int i = 0; i < m_data->m_dynamicsWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)m_data->m_dynamicsWorld->getSoftBodyArray()[i];
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
for (int i = 0; i < deformWorld->getSoftBodyArray().size(); i++)
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(), m_data->m_dynamicsWorld->getDrawFlags());
btSoftBody* psb = (btSoftBody*)deformWorld->getSoftBodyArray()[i];
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),deformWorld->getDrawFlags());
}
}
}
}
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
for (int i = 0; i < softWorld->getSoftBodyArray().size(); i++)
{
btSoftBody* psb = (btSoftBody*)softWorld->getSoftBodyArray()[i];
if (m_data->m_dynamicsWorld->getDebugDrawer() && !(m_data->m_dynamicsWorld->getDebugDrawer()->getDebugMode() & (btIDebugDraw::DBG_DrawWireframe)))
{
//btSoftBodyHelpers::DrawFrame(psb,m_data->m_dynamicsWorld->getDebugDrawer());
btSoftBodyHelpers::Draw(psb, m_data->m_dynamicsWorld->getDebugDrawer(),softWorld->getDrawFlags());
}
}
}
}
#endif
}
}
@@ -13272,7 +13421,7 @@ void PhysicsServerCommandProcessor::addBodyChangedNotifications()
}
}
void PhysicsServerCommandProcessor::resetSimulation()
void PhysicsServerCommandProcessor::resetSimulation(int flags)
{
//clean up all data
m_data->m_remoteSyncTransformTime = m_data->m_remoteSyncTransformInterval;
@@ -13283,7 +13432,21 @@ void PhysicsServerCommandProcessor::resetSimulation()
#ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD
if (m_data && m_data->m_dynamicsWorld)
{
m_data->m_dynamicsWorld->getWorldInfo().m_sparsesdf.Reset();
{
btDeformableMultiBodyDynamicsWorld* deformWorld = getDeformableWorld();
if (deformWorld)
{
deformWorld ->getWorldInfo().m_sparsesdf.Reset();
}
}
{
btSoftMultiBodyDynamicsWorld* softWorld = getSoftWorld();
if (softWorld)
{
softWorld->getWorldInfo().m_sparsesdf.Reset();
}
}
}
#endif
if (m_data && m_data->m_guiHelper)
@@ -13314,7 +13477,7 @@ void PhysicsServerCommandProcessor::resetSimulation()
removePickingConstraint();
deleteDynamicsWorld();
createEmptyDynamicsWorld();
createEmptyDynamicsWorld(flags);
m_data->m_bodyHandles.exitHandles();
m_data->m_bodyHandles.initHandles();

7
examples/SharedMemory/PhysicsServerCommandProcessor.h
View File

@@ -17,9 +17,12 @@ class PhysicsServerCommandProcessor : public CommandProcessorInterface
{
struct PhysicsServerCommandProcessorInternalData* m_data;
void resetSimulation();
void resetSimulation(int flags=0);
void createThreadPool();
class btDeformableMultiBodyDynamicsWorld* getDeformableWorld();
class btSoftMultiBodyDynamicsWorld* getSoftWorld();
protected:
bool processStateLoggingCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
bool processRequestCameraImageCommand(const struct SharedMemoryCommand& clientCmd, struct SharedMemoryStatus& serverStatusOut, char* bufferServerToClient, int bufferSizeInBytes);
@@ -114,7 +117,7 @@ public:
void createJointMotors(class btMultiBody* body);
virtual void createEmptyDynamicsWorld();
virtual void createEmptyDynamicsWorld(int flags=0);
virtual void deleteDynamicsWorld();
virtual bool connect()

7
examples/SharedMemory/SharedMemoryPublic.h
View File

@@ -563,6 +563,13 @@ enum b3NotificationType
SOFTBODY_CHANGED = 9,
};
enum b3ResetSimulationFlags
{
RESET_USE_DEFORMABLE_WORLD=1,
RESET_USE_DISCRETE_DYNAMICS_WORLD=2,
RESET_USE_SIMPLE_BROADPHASE=4,
};
struct b3BodyNotificationArgs
{
int m_bodyUniqueId;

16
examples/pybullet/examples/load_soft_body.py
View File

@@ -1,24 +1,24 @@
import pybullet as p
from time import sleep
physicsClient = p.connect(p.DIRECT)
physicsClient = p.connect(p.GUI)
p.setGravity(0, 0, -10)
planeId = p.loadURDF("plane.urdf")
boxId = p.loadURDF("cube.urdf", useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("bunny.obj")
planeId = p.loadURDF("plane.urdf", [0,0,-2])
boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True)
bunnyId = p.loadSoftBody("bunny.obj")#.obj")#.vtk")
#meshData = p.getMeshData(bunnyId)
#print("meshData=",meshData)
p.loadURDF("cube_small.urdf", [1, 0, 1])
#p.loadURDF("cube_small.urdf", [1, 0, 1])
useRealTimeSimulation = 1
if (useRealTimeSimulation):
p.setRealTimeSimulation(1)
print(p.getDynamicsInfo(planeId, -1))
print(p.getDynamicsInfo(bunnyId, 0))
#print(p.getDynamicsInfo(bunnyId, 0))
print(p.getDynamicsInfo(boxId, -1))
p.changeDynamics(boxId,-1,mass=10)
while p.isConnected():
p.setGravity(0, 0, -10)
if (useRealTimeSimulation):

14
examples/pybullet/pybullet.c
View File

@@ -2050,10 +2050,11 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject*
static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObject* keywds)
{
int physicsClientId = 0;
static char* kwlist[] = {"physicsClientId", NULL};
int flags = 0;
static char* kwlist[] = {"flags", "physicsClientId", NULL};
b3PhysicsClientHandle sm = 0;
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|i", kwlist, &physicsClientId))
if (!PyArg_ParseTupleAndKeywords(args, keywds, "|ii", kwlist, &flags, &physicsClientId))
{
return NULL;
}
@@ -2065,9 +2066,12 @@ static PyObject* pybullet_resetSimulation(PyObject* self, PyObject* args, PyObje
}
{
b3SharedMemoryCommandHandle commandHandle;
b3SharedMemoryStatusHandle statusHandle;
commandHandle = b3InitResetSimulationCommand(sm);
b3InitResetSimulationSetFlags(commandHandle, flags);
statusHandle = b3SubmitClientCommandAndWaitStatus(
sm, b3InitResetSimulationCommand(sm));
sm, commandHandle);
}
Py_INCREF(Py_None);
return Py_None;
@@ -12376,6 +12380,10 @@ initpybullet(void)
//PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_NNCF",eConstraintSolverLCP_NNCG);
//PyModule_AddIntConstant(m, "CONSTRAINT_SOLVER_LCP_BLOCK",eConstraintSolverLCP_BLOCK_PGS);
PyModule_AddIntConstant(m, "RESET_USE_DEFORMABLE_WORLD", RESET_USE_DEFORMABLE_WORLD);
PyModule_AddIntConstant(m, "RESET_USE_DISCRETE_DYNAMICS_WORLD", RESET_USE_DISCRETE_DYNAMICS_WORLD);
PyModule_AddIntConstant(m, "RESET_USE_SIMPLE_BROADPHASE", RESET_USE_SIMPLE_BROADPHASE);
PyModule_AddIntConstant(m, "VR_BUTTON_IS_DOWN", eButtonIsDown);
PyModule_AddIntConstant(m, "VR_BUTTON_WAS_TRIGGERED", eButtonTriggered);
PyModule_AddIntConstant(m, "VR_BUTTON_WAS_RELEASED", eButtonReleased);

47
src/BulletSoftBody/btSoftBodyHelpers.cpp
View File

@@ -1509,28 +1509,31 @@ void btSoftBodyHelpers::readRenderMeshFromObj(const char* file, btSoftBody* psb)
while (std::getline(fs, line))
{
std::stringstream ss(line);
if (line[0] == 'v' && line[1] != 't' && line[1] != 'n')
{
ss.ignore();
for (size_t i = 0; i < 3; i++)
ss >> pos[i];
btSoftBody::Node n;
n.m_x = pos;
psb->m_renderNodes.push_back(n);
}
else if (line[0] == 'f')
{
ss.ignore();
int id0, id1, id2;
ss >> id0;
ss >> id1;
ss >> id2;
btSoftBody::Face f;
f.m_n[0] = &psb->m_renderNodes[id0-1];
f.m_n[1] = &psb->m_renderNodes[id1-1];
f.m_n[2] = &psb->m_renderNodes[id2-1];
psb->m_renderFaces.push_back(f);
}
if (line.length()>1)
{
if (line[0] == 'v' && line[1] != 't' && line[1] != 'n')
{
ss.ignore();
for (size_t i = 0; i < 3; i++)
ss >> pos[i];
btSoftBody::Node n;
n.m_x = pos;
psb->m_renderNodes.push_back(n);
}
else if (line[0] == 'f')
{
ss.ignore();
int id0, id1, id2;
ss >> id0;
ss >> id1;
ss >> id2;
btSoftBody::Face f;
f.m_n[0] = &psb->m_renderNodes[id0-1];
f.m_n[1] = &psb->m_renderNodes[id1-1];
f.m_n[2] = &psb->m_renderNodes[id2-1];
psb->m_renderFaces.push_back(f);
}
}
}
fs.close();
}