From 49b098854efb9b839ea816d822197b55323e033d Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Wed, 7 Nov 2018 09:29:19 -0800 Subject: [PATCH] PyBullet: avoid calling syncBodies for each DOF in pdControl. Allow PD_CONTROL in setJointMotorControlArray. --- .../SharedMemory/PhysicsServerCommandProcessor.cpp | 5 +++++ .../plugins/pdControlPlugin/pdControlPlugin.cpp | 6 +++++- examples/pybullet/pybullet.c | 14 ++++++-------- 3 files changed, 16 insertions(+), 9 deletions(-) diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 2a48843ed..2e0fb0b94 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5581,6 +5581,11 @@ bool PhysicsServerCommandProcessor::processSendDesiredStateCommand(const struct args.m_ints[1] = bodyUniqueId; //find the joint motors and apply the desired velocity and maximum force/torque { + args.m_numInts = 0; + args.m_numFloats = 0; + //syncBodies is expensive/slow, use it only once + m_data->m_pluginManager.executePluginCommand(m_data->m_pdControlPlugin, &args); + int velIndex = 6; //skip the 3 linear + 3 angular degree of freedom velocity entries of the base int posIndex = 7; //skip 3 positional and 4 orientation (quaternion) positional degrees of freedom of the base for (int link = 0; link < mb->getNumLinks(); link++) diff --git a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp index 942b87f3f..5fa29e137 100644 --- a/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp +++ b/examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp @@ -89,7 +89,11 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c { MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer; - obj->m_api.syncBodies(); + if (arguments->m_numInts == 0) + { + obj->m_api.syncBodies(); + return -1; + } int numObj = obj->m_api.getNumBodies(); //printf("numObj = %d\n", numObj); diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index c025ee9cd..375b41a0f 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -2039,7 +2039,7 @@ static PyObject* pybullet_setJointMotorControl(PyObject* self, PyObject* args) (controlMode != CONTROL_MODE_TORQUE) && (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); + PyErr_SetString(SpamError, "Illegal control mode."); return NULL; } @@ -2140,9 +2140,10 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar if ((controlMode != CONTROL_MODE_VELOCITY) && (controlMode != CONTROL_MODE_TORQUE) && - (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)) + (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD)&& + (controlMode != CONTROL_MODE_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); + PyErr_SetString(SpamError, "Illegal control mode."); return NULL; } @@ -2340,7 +2341,7 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar break; } - case CONTROL_MODE_POSITION_VELOCITY_PD: + default: { b3JointControlSetDesiredPosition(commandHandle, info.m_qIndex, targetPosition); @@ -2351,9 +2352,6 @@ static PyObject* pybullet_setJointMotorControlArray(PyObject* self, PyObject* ar b3JointControlSetMaximumForce(commandHandle, info.m_uIndex, force); break; } - default: - { - } }; } @@ -2440,7 +2438,7 @@ static PyObject* pybullet_setJointMotorControl2(PyObject* self, PyObject* args, (controlMode != CONTROL_MODE_POSITION_VELOCITY_PD) && (controlMode != CONTROL_MODE_PD)) { - PyErr_SetString(SpamError, "Illegral control mode."); + PyErr_SetString(SpamError, "Illegal control mode."); return NULL; }