From a86710c5b6209946262c08a6d975e25fbbeffb48 Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Fri, 15 Nov 2019 21:25:11 -0800 Subject: [PATCH] add python binding to allow loading deformable objects --- examples/SharedMemory/PhysicsClientC_API.cpp | 13 ++++++++-- examples/SharedMemory/PhysicsClientC_API.h | 3 ++- .../PhysicsServerCommandProcessor.cpp | 3 +-- examples/SharedMemory/SharedMemoryCommands.h | 6 +++-- .../pybullet/examples/deformable_torus.py | 17 ++++++++++++ examples/pybullet/pybullet.c | 26 +++++++++++++++++-- 6 files changed, 59 insertions(+), 9 deletions(-) create mode 100644 examples/pybullet/examples/deformable_torus.py diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 5a5d3ad95..0f6f255fb 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -386,12 +386,12 @@ B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle return 0; } -B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision) +B3_SHARED_API int b3LoadSoftBodyUseSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision) { struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); command->m_loadSoftBodyArguments.m_useSelfCollision = useSelfCollision; - command->m_updateFlags |= LOAD_SOFT_BODY_SET_SELF_COLLISION; + command->m_updateFlags |= LOAD_SOFT_BODY_USE_SELF_COLLISION; return 0; } @@ -413,6 +413,15 @@ B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle co return 0; } +B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*)commandHandle; + b3Assert(command->m_type == CMD_LOAD_SOFT_BODY); + command->m_loadSoftBodyArguments.m_useFaceContact = useFaceContact; + command->m_updateFlags |= LOAD_SOFT_BODY_USE_FACE_CONTACT; + return 0; +} + B3_SHARED_API b3SharedMemoryCommandHandle b3LoadUrdfCommandInit(b3PhysicsClientHandle physClient, const char* urdfFileName) { PhysicsClient* cl = (PhysicsClient*)physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 49e5848f2..d2037d02e 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -638,7 +638,8 @@ extern "C" B3_SHARED_API int b3LoadSoftBodyAddMassSpringForce(b3SharedMemoryCommandHandle commandHandle, double springElasticStiffness , double springDampingStiffness); B3_SHARED_API int b3LoadSoftBodyAddGravityForce(b3SharedMemoryCommandHandle commandHandle, double gravityX, double gravityY, double gravityZ); B3_SHARED_API int b3LoadSoftBodySetCollisionHardness(b3SharedMemoryCommandHandle commandHandle, double collisionHardness); - B3_SHARED_API int b3LoadSoftBodySetSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); + B3_SHARED_API int b3LoadSoftBodyUseSelfCollision(b3SharedMemoryCommandHandle commandHandle, int useSelfCollision); + B3_SHARED_API int b3LoadSoftBodyUseFaceContact(b3SharedMemoryCommandHandle commandHandle, int useFaceContact); B3_SHARED_API int b3LoadSoftBodySetFrictionCoefficient(b3SharedMemoryCommandHandle commandHandle, double frictionCoefficient); B3_SHARED_API int b3LoadSoftBodyUseBendingSprings(b3SharedMemoryCommandHandle commandHandle, int useBendingSprings); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index f28e722c3..ac9db5e10 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8248,7 +8248,7 @@ bool PhysicsServerCommandProcessor::processLoadSoftBodyCommand(const struct Shar psb->setCollisionFlags(0); psb->setTotalMass(mass); bool use_self_collision = false; - if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_SET_SELF_COLLISION) + if (clientCmd.m_updateFlags & LOAD_SOFT_BODY_USE_SELF_COLLISION) { use_self_collision = loadSoftBodyArgs.m_useSelfCollision; } @@ -13444,7 +13444,6 @@ void PhysicsServerCommandProcessor::resetSimulation(int flags) softWorld->getWorldInfo().m_sparsesdf.Reset(); } } - } #endif if (m_data && m_data->m_guiHelper) diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 610bfe275..4d4324013 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -502,7 +502,8 @@ enum EnumLoadSoftBodyUpdateFlags LOAD_SOFT_BODY_SET_FRICTION_COEFFICIENT = 1<<10, LOAD_SOFT_BODY_ADD_BENDING_SPRINGS = 1<<11, LOAD_SOFT_BODY_ADD_NEOHOOKEAN_FORCE = 1<<12, - LOAD_SOFT_BODY_SET_SELF_COLLISION = 1<<13, + LOAD_SOFT_BODY_USE_SELF_COLLISION = 1<<13, + LOAD_SOFT_BODY_USE_FACE_CONTACT = 1<<14, }; enum EnumSimParamInternalSimFlags @@ -525,13 +526,14 @@ struct LoadSoftBodyArgs double m_springDampingStiffness; double m_corotatedMu; double m_corotatedLambda; - bool m_useBendingSprings; + int m_useBendingSprings; double m_collisionHardness; double m_useSelfCollision; double m_frictionCoeff; double m_NeoHookeanMu; double m_NeoHookeanLambda; double m_NeoHookeanDamping; + int m_useFaceContact; }; struct b3LoadSoftBodyResultArgs diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py new file mode 100644 index 000000000..8bebe38ab --- /dev/null +++ b/examples/pybullet/examples/deformable_torus.py @@ -0,0 +1,17 @@ +import pybullet as p +from time import sleep + +physicsClient = p.connect(p.GUI) + +p.resetSimulation(p.RESET_USE_DEFORMABLE_WORLD) + +p.setGravity(0, 0, -10) + +planeId = p.loadURDF("plane.urdf", [0,0,-2]) + +boxId = p.loadURDF("cube.urdf", [0,3,2],useMaximalCoordinates = True) + +bunnyId = p.loadSoftBody("torus.vtk", useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, useSelfCollision = 1, frictionCoeff = 0.5) +p.setGravity(0, 0, -10) +while p.isConnected(): + p.stepSimulation() diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 6e673bc1b..0b1ef28cf 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1963,13 +1963,25 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* int physicsClientId = 0; int flags = 0; - static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL}; int bodyUniqueId = -1; const char* fileName = ""; double scale = -1; double mass = -1; double collisionMargin = -1; + int useMassSpring = 0; + int useBendingSprings = 0; + int useNeoHookean = 0; + double springElasticStiffness = 1; + double springDampingStiffness = 0.1; + double NeoHookeanMu = 1; + double NeoHookeanLambda = 1; + double NeoHookeanDamping = 0.1; + double frictionCoeff = 0; + int useFaceContact = 0; + int useSelfCollision = 0; + b3PhysicsClientHandle sm = 0; @@ -1980,7 +1992,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* PyObject* basePosObj = 0; PyObject* baseOrnObj = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddi", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiiddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision)) { return NULL; } @@ -2033,6 +2045,16 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* { b3LoadSoftBodySetCollisionMargin(command, collisionMargin); } + if (useMassSpring) + { + b3LoadSoftBodyAddMassSpringForce(command, springElasticStiffness, springDampingStiffness); + b3LoadSoftBodyUseBendingSprings(command, useBendingSprings); + } + if (useNeoHookean) + { + b3LoadSoftBodyAddNeoHookeanForce(command, NeoHookeanMu, NeoHookeanLambda, NeoHookeanDamping); + } + b3LoadSoftBodySetFrictionCoefficient(command, frictionCoeff); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); if (statusType != CMD_LOAD_SOFT_BODY_COMPLETED)