From c178c101a853115d6ba8b1a03444c5e886ce4926 Mon Sep 17 00:00:00 2001 From: Erwin Coumans Date: Wed, 18 Oct 2017 19:15:35 -0700 Subject: [PATCH] fix the pybullet.changeDynamics linear/angular damping expose pybullet.getConstraintState --- examples/SharedMemory/PhysicsClientC_API.cpp | 42 +++++++++++++- examples/SharedMemory/PhysicsClientC_API.h | 5 ++ .../PhysicsClientSharedMemory.cpp | 4 ++ examples/SharedMemory/PhysicsDirect.cpp | 5 +- .../PhysicsServerCommandProcessor.cpp | 44 ++++++++++++--- examples/SharedMemory/SharedMemoryCommands.h | 3 +- examples/SharedMemory/SharedMemoryPublic.h | 11 +++- examples/pybullet/pybullet.c | 56 ++++++++++++++++++- 8 files changed, 154 insertions(+), 16 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 58a251949..b27d7b6f2 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -2128,7 +2128,7 @@ B3_SHARED_API int b3ChangeDynamicsInfoSetAngularDamping(b3SharedMemoryCommandHan struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; b3Assert(command->m_type == CMD_CHANGE_DYNAMICS_INFO); command->m_changeDynamicsInfoArgs.m_bodyUniqueId = bodyUniqueId; - command->m_changeDynamicsInfoArgs.m_linearDamping = angularDamping; + command->m_changeDynamicsInfoArgs.m_angularDamping = angularDamping; command->m_updateFlags |= CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING; return 0; } @@ -2292,10 +2292,47 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c } +B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId) +{ + PhysicsClient* cl = (PhysicsClient*)physClient; + b3Assert(cl); + b3Assert(cl->canSubmitCommand()); + struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); + b3Assert(command); + + command->m_type = CMD_USER_CONSTRAINT; + command->m_updateFlags = USER_CONSTRAINT_REQUEST_STATE; + command->m_userConstraintArguments.m_userConstraintUniqueId = constraintUniqueId; + return (b3SharedMemoryCommandHandle)command; +} + +B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus*)statusHandle; + if (status) + { + btAssert(status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED); + if (status && status->m_type == CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED) + { + int i = 0; + constraintState->m_numDofs = status->m_userConstraintStateResultArgs.m_numDofs; + for (i = 0; i < constraintState->m_numDofs; i++) + { + constraintState->m_appliedConstraintForces[i] = status->m_userConstraintStateResultArgs.m_appliedConstraintForces[i]; + } + for (; i < 6; i++) + { + constraintState->m_appliedConstraintForces[i] = 0; + } + return 1; + } + } + return 0; +} B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId) { - PhysicsClient* cl = (PhysicsClient* ) physClient; + PhysicsClient* cl = (PhysicsClient* ) physClient; b3Assert(cl); b3Assert(cl->canSubmitCommand()); struct SharedMemoryCommand* command = cl->getAvailableSharedMemoryCommand(); @@ -2341,6 +2378,7 @@ B3_SHARED_API int b3GetStatusUserConstraintUniqueId(b3SharedMemoryStatusHandle s } + B3_SHARED_API b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, double rayToWorldX, double rayToWorldY, double rayToWorldZ) diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 539dacfa5..979fb9e14 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -143,6 +143,11 @@ B3_SHARED_API int b3InitChangeUserConstraintSetERP(b3SharedMemoryCommandHandle c B3_SHARED_API b3SharedMemoryCommandHandle b3InitRemoveUserConstraintCommand(b3PhysicsClientHandle physClient, int userConstraintUniqueId); B3_SHARED_API int b3GetNumUserConstraints(b3PhysicsClientHandle physClient); + +B3_SHARED_API b3SharedMemoryCommandHandle b3InitGetUserConstraintStateCommand(b3PhysicsClientHandle physClient, int constraintUniqueId); +B3_SHARED_API int b3GetStatusUserConstraintState(b3SharedMemoryStatusHandle statusHandle, struct b3UserConstraintState* constraintState); + + B3_SHARED_API int b3GetUserConstraintInfo(b3PhysicsClientHandle physClient, int constraintUniqueId, struct b3UserConstraint* info); /// return the user constraint id, given the index in range [0 , b3GetNumUserConstraints() ) B3_SHARED_API int b3GetUserConstraintId(b3PhysicsClientHandle physClient, int serialIndex); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 20eaf0ad5..eff0efe92 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -601,6 +601,10 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { break; } + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } case CMD_USER_CONSTRAINT_INFO_COMPLETED: { B3_PROFILE("CMD_USER_CONSTRAINT_INFO_COMPLETED"); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 9ad84404e..cb8db2f35 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -783,7 +783,10 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd } break; } - + case CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED: + { + break; + } case CMD_SYNC_BODY_INFO_COMPLETED: case CMD_MJCF_LOADING_COMPLETED: case CMD_SDF_LOADING_COMPLETED: diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index d3cfa2d5b..6c4461d4e 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5630,13 +5630,22 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm double rollingFriction = clientCmd.m_changeDynamicsInfoArgs.m_rollingFriction; double restitution = clientCmd.m_changeDynamicsInfoArgs.m_restitution; btAssert(bodyUniqueId >= 0); - btAssert(linkIndex >= -1); InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId); if (body && body->m_multiBody) { btMultiBody* mb = body->m_multiBody; + + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) + { + mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); + } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) + { + mb->setAngularDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); + } + if (linkIndex == -1) { if (mb->getBaseCollider()) @@ -5645,14 +5654,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm { mb->getBaseCollider()->setRestitution(restitution); } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_LINEAR_DAMPING) - { - mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_linearDamping); - } - if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_ANGULAR_DAMPING) - { - mb->setLinearDamping(clientCmd.m_changeDynamicsInfoArgs.m_angularDamping); - } + if (clientCmd.m_updateFlags & CHANGE_DYNAMICS_INFO_SET_CONTACT_STIFFNESS_AND_DAMPING) { @@ -7173,6 +7175,29 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm SharedMemoryStatus& serverCmd =serverStatusOut; serverCmd.m_type = CMD_USER_CONSTRAINT_FAILED; hasStatus = true; + if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_STATE) + { + int constraintUid = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; + InteralUserConstraintData* userConstraintPtr = m_data->m_userConstraints.find(constraintUid); + if (userConstraintPtr) + { + serverCmd.m_userConstraintStateResultArgs.m_numDofs = 0; + for (int i = 0; i < 6; i++) + { + serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = 0; + } + if (userConstraintPtr->m_mbConstraint) + { + serverCmd.m_userConstraintStateResultArgs.m_numDofs = userConstraintPtr->m_mbConstraint->getNumRows(); + for (int i = 0; i < userConstraintPtr->m_mbConstraint->getNumRows(); i++) + { + serverCmd.m_userConstraintStateResultArgs.m_appliedConstraintForces[i] = userConstraintPtr->m_mbConstraint->getAppliedImpulse(i) / m_data->m_dynamicsWorld->getSolverInfo().m_timeStep; + } + serverCmd.m_type = CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED; + } + } + + }; if (clientCmd.m_updateFlags & USER_CONSTRAINT_REQUEST_INFO) { int userConstraintUidChange = clientCmd.m_userConstraintArguments.m_userConstraintUniqueId; @@ -7180,6 +7205,7 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm if (userConstraintPtr) { serverCmd.m_userConstraintResultArgs = userConstraintPtr->m_userConstraintData; + serverCmd.m_type = CMD_USER_CONSTRAINT_INFO_COMPLETED; } } diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index c4c82762d..3afe9eae0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -676,7 +676,7 @@ enum EnumUserConstraintFlags USER_CONSTRAINT_CHANGE_GEAR_AUX_LINK=256, USER_CONSTRAINT_CHANGE_RELATIVE_POSITION_TARGET=512, USER_CONSTRAINT_CHANGE_ERP=1024, - + USER_CONSTRAINT_REQUEST_STATE=2048, }; enum EnumBodyChangeFlags @@ -1039,6 +1039,7 @@ struct SharedMemoryStatus struct SendVisualShapeDataArgs m_sendVisualShapeArgs; struct UserDebugDrawResultArgs m_userDebugDrawArgs; struct b3UserConstraint m_userConstraintResultArgs; + struct b3UserConstraintState m_userConstraintStateResultArgs; struct SendVREvents m_sendVREvents; struct SendKeyboardEvents m_sendKeyboardEvents; struct SendRaycastHits m_raycastHits; diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index b3b223ae1..6a069f6de 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -5,7 +5,8 @@ ///increase the SHARED_MEMORY_MAGIC_NUMBER whenever incompatible changes are made in the structures ///my convention is year/month/day/rev -#define SHARED_MEMORY_MAGIC_NUMBER 201710050 +#define SHARED_MEMORY_MAGIC_NUMBER 201710180 +//#define SHARED_MEMORY_MAGIC_NUMBER 201710050 //#define SHARED_MEMORY_MAGIC_NUMBER 201708270 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015 @@ -143,6 +144,7 @@ enum EnumSharedMemoryServerStatus CMD_USER_DEBUG_DRAW_FAILED, CMD_USER_CONSTRAINT_COMPLETED, CMD_USER_CONSTRAINT_INFO_COMPLETED, + CMD_USER_CONSTRAINT_REQUEST_STATE_COMPLETED, CMD_REMOVE_USER_CONSTRAINT_COMPLETED, CMD_CHANGE_USER_CONSTRAINT_COMPLETED, CMD_REMOVE_USER_CONSTRAINT_FAILED, @@ -253,7 +255,6 @@ struct b3UserConstraint int m_gearAuxLink; double m_relativePositionTarget; double m_erp; - }; struct b3BodyInfo @@ -331,6 +332,12 @@ struct b3OpenGLVisualizerCameraInfo float m_target[3]; }; +struct b3UserConstraintState +{ + double m_appliedConstraintForces[6]; + int m_numDofs; +}; + enum b3VREventType { VR_CONTROLLER_MOVE_EVENT=1, diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index 508e6c461..e96d24218 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2470,7 +2470,7 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"constraintUniqueId", "physicsClientId", NULL}; + static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL }; if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId)) { return NULL; @@ -2539,10 +2539,61 @@ static PyObject* pybullet_getConstraintInfo(PyObject* self, PyObject* args, PyOb } } + PyErr_SetString(SpamError, "Couldn't get user constraint info"); return NULL; } +static PyObject* pybullet_getConstraintState(PyObject* self, PyObject* args, PyObject* keywds) +{ + { + int constraintUniqueId = -1; + b3PhysicsClientHandle sm = 0; + + int physicsClientId = 0; + static char* kwlist[] = { "constraintUniqueId", "physicsClientId", NULL }; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "i|i", kwlist, &constraintUniqueId, &physicsClientId)) + { + return NULL; + } + sm = getPhysicsClient(physicsClientId); + if (sm == 0) + { + PyErr_SetString(SpamError, "Not connected to physics server."); + return NULL; + } + + { + b3SharedMemoryCommandHandle cmd_handle; + b3SharedMemoryStatusHandle statusHandle; + int statusType; + if (b3CanSubmitCommand(sm)) + { + struct b3UserConstraintState constraintState; + cmd_handle = b3InitGetUserConstraintStateCommand(sm, constraintUniqueId); + statusHandle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle); + statusType = b3GetStatusType(statusHandle); + + if (b3GetStatusUserConstraintState(statusHandle, &constraintState)) + { + if (constraintState.m_numDofs) + { + PyObject* appliedConstraintForces = PyTuple_New(constraintState.m_numDofs); + int i = 0; + for (i = 0; i < constraintState.m_numDofs; i++) + { + PyTuple_SetItem(appliedConstraintForces, i, PyFloat_FromDouble(constraintState.m_appliedConstraintForces[i])); + } + return appliedConstraintForces; + } + } + } + } + } + PyErr_SetString(SpamError, "Couldn't getConstraintState."); + return NULL; +} + static PyObject* pybullet_getConstraintUniqueId(PyObject* self, PyObject* args, PyObject* keywds) { int physicsClientId = 0; @@ -7699,6 +7750,9 @@ static PyMethodDef SpamMethods[] = { {"getConstraintInfo", (PyCFunction)pybullet_getConstraintInfo, METH_VARARGS | METH_KEYWORDS, "Get the user-created constraint info, given a constraint unique id."}, + { "getConstraintState", (PyCFunction)pybullet_getConstraintState, METH_VARARGS | METH_KEYWORDS, + "Get the user-created constraint state (applied forces), given a constraint unique id." }, + {"getConstraintUniqueId", (PyCFunction)pybullet_getConstraintUniqueId, METH_VARARGS | METH_KEYWORDS, "Get the unique id of the constraint, given a integer index in range [0.. number of constraints)."},