From b1f8eb74a40215074254b404ded25d7c59b89e8b Mon Sep 17 00:00:00 2001 From: erwincoumans Date: Tue, 26 Sep 2017 10:05:17 -0700 Subject: [PATCH] bump up shared memory version number add option to recompute forward kinematics, to be consistent with link velocities in pybullet.getLinkState (..., computeForwardKinematics=0/1), thanks to Jeff Bingham for bringing up this inconsistency --- examples/SharedMemory/PhysicsClientC_API.cpp | 12 +++++++++ examples/SharedMemory/PhysicsClientC_API.h | 2 ++ .../PhysicsClientSharedMemory.cpp | 9 +++++++ examples/SharedMemory/PhysicsDirect.cpp | 26 ++++++++++++++++++- .../PhysicsServerCommandProcessor.cpp | 11 ++++++++ examples/SharedMemory/SharedMemoryPublic.h | 6 +++-- examples/pybullet/pybullet.c | 10 +++++-- 7 files changed, 71 insertions(+), 5 deletions(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 1ad7c41c1..1270a8021 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -649,6 +649,18 @@ B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryC return 0; } +B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics) +{ + struct SharedMemoryCommand* command = (struct SharedMemoryCommand*) commandHandle; + b3Assert(command); + btAssert(command->m_type == CMD_REQUEST_ACTUAL_STATE); + if (computeForwardKinematics && command->m_type == CMD_REQUEST_ACTUAL_STATE) + { + command->m_updateFlags |= ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS; + } + return 0; +} + B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, b3JointSensorState *state) { diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index 4b686cec0..d2b2c8421 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -439,6 +439,8 @@ B3_SHARED_API int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle com B3_SHARED_API b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); B3_SHARED_API int b3RequestActualStateCommandComputeLinkVelocity(b3SharedMemoryCommandHandle commandHandle, int computeLinkVelocity); +B3_SHARED_API int b3RequestActualStateCommandComputeForwardKinematics(b3SharedMemoryCommandHandle commandHandle, int computeForwardKinematics); + B3_SHARED_API int b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); B3_SHARED_API int b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state); diff --git a/examples/SharedMemory/PhysicsClientSharedMemory.cpp b/examples/SharedMemory/PhysicsClientSharedMemory.cpp index 1ea576f65..94c73f5f2 100644 --- a/examples/SharedMemory/PhysicsClientSharedMemory.cpp +++ b/examples/SharedMemory/PhysicsClientSharedMemory.cpp @@ -1204,6 +1204,15 @@ const SharedMemoryStatus* PhysicsClientSharedMemory::processServerStatus() { { break; } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } case CMD_CUSTOM_COMMAND_FAILED: { b3Warning("custom plugin command failed"); diff --git a/examples/SharedMemory/PhysicsDirect.cpp b/examples/SharedMemory/PhysicsDirect.cpp index 2de1acabd..76d09dfe7 100644 --- a/examples/SharedMemory/PhysicsDirect.cpp +++ b/examples/SharedMemory/PhysicsDirect.cpp @@ -949,7 +949,31 @@ void PhysicsDirect::postProcessStatus(const struct SharedMemoryStatus& serverCmd b3Warning("custom plugin command failed"); break; } - + case CMD_CLIENT_COMMAND_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_COMPLETED: + { + break; + } + case CMD_CALCULATED_JACOBIAN_FAILED: + { + b3Warning("jacobian calculation failed"); + break; + } + case CMD_ACTUAL_STATE_UPDATE_COMPLETED: + { + break; + } + case CMD_DESIRED_STATE_RECEIVED_COMPLETED: + { + break; + } + case CMD_STEP_FORWARD_SIMULATION_COMPLETED: + { + break; + } default: { //b3Warning("Unknown server status type"); diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 970967b40..012169251 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -5205,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm btAlignedObjectArray omega; btAlignedObjectArray linVel; + bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0); + if (computeForwardKinematics) + { + B3_PROFILE("compForwardKinematics"); + btAlignedObjectArray world_to_local; + btAlignedObjectArray local_origin; + world_to_local.resize(mb->getNumLinks()+1); + local_origin.resize(mb->getNumLinks()+1); + mb->forwardKinematics(world_to_local,local_origin); + } + bool computeLinkVelocities = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_LINKVELOCITY)!=0); if (computeLinkVelocities) { diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 2186f3a78..90dbfe8b9 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 201708270 +#define SHARED_MEMORY_MAGIC_NUMBER 201709260 +//#define SHARED_MEMORY_MAGIC_NUMBER 201708270 //#define SHARED_MEMORY_MAGIC_NUMBER 201707140 //#define SHARED_MEMORY_MAGIC_NUMBER 201706015 //#define SHARED_MEMORY_MAGIC_NUMBER 201706001 @@ -505,7 +506,8 @@ struct b3VisualShapeInformation enum eLinkStateFlags { - ACTUAL_STATE_COMPUTE_LINKVELOCITY=1 + ACTUAL_STATE_COMPUTE_LINKVELOCITY=1, + ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS=2, }; ///b3LinkState provides extra information such as the Cartesian world coordinates diff --git a/examples/pybullet/pybullet.c b/examples/pybullet/pybullet.c index aa6c4c63c..873430e8a 100644 --- a/examples/pybullet/pybullet.c +++ b/examples/pybullet/pybullet.c @@ -3126,13 +3126,14 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* int bodyUniqueId = -1; int linkIndex = -1; int computeLinkVelocity = 0; + int computeForwardKinematics = 0; int i; b3PhysicsClientHandle sm = 0; int physicsClientId = 0; - static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "physicsClientId", NULL}; - if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity, &physicsClientId)) + static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "computeForwardKinematics", "physicsClientId", NULL}; + if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|iii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity,&computeForwardKinematics,&physicsClientId)) { return NULL; } @@ -3168,6 +3169,11 @@ static PyObject* pybullet_getLinkState(PyObject* self, PyObject* args, PyObject* b3RequestActualStateCommandComputeLinkVelocity(cmd_handle,computeLinkVelocity); } + if (computeForwardKinematics) + { + b3RequestActualStateCommandComputeForwardKinematics(cmd_handle,computeForwardKinematics); + } + status_handle = b3SubmitClientCommandAndWaitStatus(sm, cmd_handle);