From 5f1260199aa89006461e3fb6ff29b9f8f75436cf Mon Sep 17 00:00:00 2001 From: Xuchen Han Date: Mon, 27 Apr 2020 19:51:12 -0700 Subject: [PATCH] set appropriate repulsion stiffness for pybullet torus demo --- data/torus_deform.urdf | 1 + examples/SharedMemory/PhysicsClientC_API.h | 2 +- examples/pybullet/examples/deformable_torus.py | 3 +-- examples/pybullet/pybullet.c | 9 +++++++-- src/btLinearMathAll.cpp | 1 + 5 files changed, 11 insertions(+), 5 deletions(-) diff --git a/data/torus_deform.urdf b/data/torus_deform.urdf index c5d661993..7d09ac281 100644 --- a/data/torus_deform.urdf +++ b/data/torus_deform.urdf @@ -6,6 +6,7 @@ + diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index b3d077a28..438a4b3c0 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -648,7 +648,7 @@ extern "C" 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 b3LoadSoftBodyRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness); + B3_SHARED_API int b3LoadSoftBodySetRepulsionStiffness(b3SharedMemoryCommandHandle commandHandle, double stiffness); 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, double bendingStiffness); diff --git a/examples/pybullet/examples/deformable_torus.py b/examples/pybullet/examples/deformable_torus.py index eedc1ea83..8416b651c 100644 --- a/examples/pybullet/examples/deformable_torus.py +++ b/examples/pybullet/examples/deformable_torus.py @@ -11,7 +11,7 @@ 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) +bunnyId = p.loadSoftBody("torus.vtk", mass = 1, useNeoHookean = 1, NeoHookeanMu = 60, NeoHookeanLambda = 200, NeoHookeanDamping = 0.01, collisionMargin = 0.006, useSelfCollision = 1, frictionCoeff = 0.5, repulsionStiffness = 50) bunny2 = p.loadURDF("torus_deform.urdf", [0,1,0], flags=p.URDF_USE_SELF_COLLISION) @@ -20,4 +20,3 @@ p.setRealTimeSimulation(1) while p.isConnected(): p.setGravity(0,0,-10) - sleep(1./240.) diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index d6beded74..b8c2ee906 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2038,7 +2038,7 @@ 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", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", NULL}; + static char* kwlist[] = {"fileName", "basePosition", "baseOrientation", "scale", "mass", "collisionMargin", "physicsClientId", "useMassSpring", "useBendingSprings", "useNeoHookean", "springElasticStiffness", "springDampingStiffness", "springBendingStiffness", "NeoHookeanMu", "NeoHookeanLambda", "NeoHookeanDamping", "frictionCoeff", "useFaceContact", "useSelfCollision", "repulsionStiffness", NULL}; int bodyUniqueId = -1; const char* fileName = ""; @@ -2057,6 +2057,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* double frictionCoeff = 0; int useFaceContact = 0; int useSelfCollision = 0; + double repulsionStiffness = 0.5; b3PhysicsClientHandle sm = 0; @@ -2068,7 +2069,7 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* PyObject* basePosObj = 0; PyObject* baseOrnObj = 0; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiidddddddii", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision)) + if (!PyArg_ParseTupleAndKeywords(args, keywds, "s|OOdddiiiidddddddiid", kwlist, &fileName, &basePosObj, &baseOrnObj, &scale, &mass, &collisionMargin, &physicsClientId, &useMassSpring, &useBendingSprings, &useNeoHookean, &springElasticStiffness, &springDampingStiffness, &springBendingStiffness, &NeoHookeanMu, &NeoHookeanLambda, &NeoHookeanDamping, &frictionCoeff, &useFaceContact, &useSelfCollision, &repulsionStiffness)) { return NULL; } @@ -2134,6 +2135,10 @@ static PyObject* pybullet_loadSoftBody(PyObject* self, PyObject* args, PyObject* { b3LoadSoftBodySetSelfCollision(command, useSelfCollision); } + if (repulsionStiffness > 0) + { + b3LoadSoftBodySetRepulsionStiffness(command, repulsionStiffness); + } b3LoadSoftBodySetFrictionCoefficient(command, frictionCoeff); statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command); statusType = b3GetStatusType(statusHandle); diff --git a/src/btLinearMathAll.cpp b/src/btLinearMathAll.cpp index 808f41280..d05a19e63 100644 --- a/src/btLinearMathAll.cpp +++ b/src/btLinearMathAll.cpp @@ -8,6 +8,7 @@ #include "LinearMath/btConvexHullComputer.cpp" #include "LinearMath/btQuickprof.cpp" #include "LinearMath/btThreads.cpp" +#include "LinearMath/btReducedVector.cpp" #include "LinearMath/TaskScheduler/btTaskScheduler.cpp" #include "LinearMath/TaskScheduler/btThreadSupportPosix.cpp" #include "LinearMath/TaskScheduler/btThreadSupportWin32.cpp"