From e4a5f9e06e257ef839fc80c226ffd6099c96b187 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 3 Sep 2019 14:27:19 -0700 Subject: [PATCH 1/2] add body type info to dynamics info --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 13 +++++++------ examples/SharedMemory/SharedMemoryPublic.h | 11 ++++++++++- examples/pybullet/examples/load_soft_body.py | 5 +++++ examples/pybullet/pybullet.c | 3 ++- 4 files changed, 24 insertions(+), 8 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 249c2ae16..09bb25988 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -8818,7 +8818,8 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - + serverCmd.m_dynamicsInfo.m_bodyType = BT_MULTI_BODY; + btMultiBody* mb = body->m_multiBody; if (linkIndex == -1) { @@ -8932,6 +8933,7 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S { SharedMemoryStatus& serverCmd = serverStatusOut; serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; + serverCmd.m_dynamicsInfo.m_bodyType = BT_RIGID_BODY; btRigidBody* rb = body->m_rigidBody; serverCmd.m_dynamicsInfo.m_lateralFrictionCoeff = rb->getFriction(); @@ -8943,11 +8945,10 @@ bool PhysicsServerCommandProcessor::processGetDynamicsInfoCommand(const struct S } #ifndef SKIP_SOFT_BODY_MULTI_BODY_DYNAMICS_WORLD else if (body && body->m_softBody){ - //todo: @fuchuyuan implement dynamics info - b3Warning("Softbody dynamics info not set!!!"); - SharedMemoryStatus& serverCmd = serverStatusOut; - serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; - } + SharedMemoryStatus& serverCmd = serverStatusOut; + serverCmd.m_type = CMD_GET_DYNAMICS_INFO_COMPLETED; + serverCmd.m_dynamicsInfo.m_bodyType = BT_SOFT_BODY; + } #endif return hasStatus; } diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index c5b32b05a..78c0c0fcb 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -7,7 +7,8 @@ //Please don't replace an existing magic number: //instead, only ADD a new one at the top, comment-out previous one -#define SHARED_MEMORY_MAGIC_NUMBER 201908110 +#define SHARED_MEMORY_MAGIC_NUMBER 201909030 +//#define SHARED_MEMORY_MAGIC_NUMBER 201908110 //#define SHARED_MEMORY_MAGIC_NUMBER 201908050 //#define SHARED_MEMORY_MAGIC_NUMBER 2019060190 //#define SHARED_MEMORY_MAGIC_NUMBER 201904030 @@ -340,6 +341,13 @@ enum DynamicsActivationState eActivationStateDisableWakeup = 32, }; +enum b3BodyType +{ + BT_RIGID_BODY = 1, + BT_MULTI_BODY = 2, + BT_SOFT_BODY = 3, +}; + struct b3DynamicsInfo { double m_mass; @@ -353,6 +361,7 @@ struct b3DynamicsInfo double m_contactStiffness; double m_contactDamping; int m_activationState; + int m_bodyType; double m_angularDamping; double m_linearDamping; double m_ccdSweptSphereRadius; diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py index 401c25a92..1d1052c88 100644 --- a/examples/pybullet/examples/load_soft_body.py +++ b/examples/pybullet/examples/load_soft_body.py @@ -5,6 +5,7 @@ physicsClient = p.connect(p.DIRECT) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") +boxId = p.loadURDF("cube.urdf", useMaximalCoordinates = True) bunnyId = p.loadSoftBody("bunny.obj") #meshData = p.getMeshData(bunnyId) #print("meshData=",meshData) @@ -14,6 +15,10 @@ useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) +print(p.getDynamicsInfo(planeId, -1)) +print(p.getDynamicsInfo(bunnyId, 0)) +print(p.getDynamicsInfo(boxId, -1)) + while p.isConnected(): p.setGravity(0, 0, -10) if (useRealTimeSimulation): diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index e7a799c32..45bad547e 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -1462,7 +1462,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje if (b3GetDynamicsInfo(status_handle, &info)) { - int numFields = 10; + int numFields = 11; PyObject* pyDynamicsInfo = PyTuple_New(numFields); PyTuple_SetItem(pyDynamicsInfo, 0, PyFloat_FromDouble(info.m_mass)); PyTuple_SetItem(pyDynamicsInfo, 1, PyFloat_FromDouble(info.m_lateralFrictionCoeff)); @@ -1494,6 +1494,7 @@ static PyObject* pybullet_getDynamicsInfo(PyObject* self, PyObject* args, PyObje PyTuple_SetItem(pyDynamicsInfo, 7, PyFloat_FromDouble(info.m_spinningFrictionCoeff)); PyTuple_SetItem(pyDynamicsInfo, 8, PyFloat_FromDouble(info.m_contactDamping)); PyTuple_SetItem(pyDynamicsInfo, 9, PyFloat_FromDouble(info.m_contactStiffness)); + PyTuple_SetItem(pyDynamicsInfo, 10, PyInt_FromLong(info.m_bodyType)); return pyDynamicsInfo; } } From ecc28d6472dd5a34a33a5ed98a9f51fbec06f4d0 Mon Sep 17 00:00:00 2001 From: Chuyuan Fu Date: Tue, 3 Sep 2019 14:35:33 -0700 Subject: [PATCH 2/2] revert testing example --- examples/pybullet/examples/load_soft_body.py | 5 ----- 1 file changed, 5 deletions(-) diff --git a/examples/pybullet/examples/load_soft_body.py b/examples/pybullet/examples/load_soft_body.py index 1d1052c88..401c25a92 100644 --- a/examples/pybullet/examples/load_soft_body.py +++ b/examples/pybullet/examples/load_soft_body.py @@ -5,7 +5,6 @@ physicsClient = p.connect(p.DIRECT) p.setGravity(0, 0, -10) planeId = p.loadURDF("plane.urdf") -boxId = p.loadURDF("cube.urdf", useMaximalCoordinates = True) bunnyId = p.loadSoftBody("bunny.obj") #meshData = p.getMeshData(bunnyId) #print("meshData=",meshData) @@ -15,10 +14,6 @@ useRealTimeSimulation = 1 if (useRealTimeSimulation): p.setRealTimeSimulation(1) -print(p.getDynamicsInfo(planeId, -1)) -print(p.getDynamicsInfo(bunnyId, 0)) -print(p.getDynamicsInfo(boxId, -1)) - while p.isConnected(): p.setGravity(0, 0, -10) if (useRealTimeSimulation):