From f99d1c45aa922e74273cb8c715d18c1814d2320d Mon Sep 17 00:00:00 2001 From: Erwin Coumans <erwincoumans@google.com> Date: Sat, 23 Apr 2016 17:29:46 -0700 Subject: [PATCH] expose multibody link world transform in the shared memory API --- examples/SharedMemory/PhysicsClientC_API.cpp | 19 +++++++++++++++++++ examples/SharedMemory/PhysicsClientC_API.h | 1 + .../PhysicsServerCommandProcessor.cpp | 11 ++++++++++- examples/SharedMemory/SharedMemoryCommands.h | 2 ++ examples/SharedMemory/SharedMemoryPublic.h | 8 ++++++++ 5 files changed, 40 insertions(+), 1 deletion(-) diff --git a/examples/SharedMemory/PhysicsClientC_API.cpp b/examples/SharedMemory/PhysicsClientC_API.cpp index 771e6eee7..b11c5ca65 100644 --- a/examples/SharedMemory/PhysicsClientC_API.cpp +++ b/examples/SharedMemory/PhysicsClientC_API.cpp @@ -222,6 +222,25 @@ void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandl } } +void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, b3LinkState *state) +{ + const SharedMemoryStatus* status = (const SharedMemoryStatus* ) statusHandle; + b3Assert(status); + int bodyIndex = status->m_sendActualStateArgs.m_bodyUniqueId; + b3Assert(bodyIndex>=0); + if (bodyIndex>=0) + { + for (int i = 0; i < 7; ++i) + { + state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i]; + } + for (int i = 0; i < 4; ++i) + { + state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i]; + } + } +} + b3SharedMemoryCommandHandle b3CreateBoxShapeCommandInit(b3PhysicsClientHandle physClient) { PhysicsClient* cl = (PhysicsClient* ) physClient; diff --git a/examples/SharedMemory/PhysicsClientC_API.h b/examples/SharedMemory/PhysicsClientC_API.h index d2fd34a5b..c6d465348 100644 --- a/examples/SharedMemory/PhysicsClientC_API.h +++ b/examples/SharedMemory/PhysicsClientC_API.h @@ -130,6 +130,7 @@ int b3CreateSensorEnableIMUForLink(b3SharedMemoryCommandHandle commandHandle, in b3SharedMemoryCommandHandle b3RequestActualStateCommandInit(b3PhysicsClientHandle physClient,int bodyUniqueId); void b3GetJointState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int jointIndex, struct b3JointSensorState *state); +void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle statusHandle, int linkIndex, struct b3LinkState *state); b3SharedMemoryCommandHandle b3PickBody(b3PhysicsClientHandle physClient, double rayFromWorldX, double rayFromWorldY, double rayFromWorldZ, diff --git a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp index 116357390..f498fccaf 100644 --- a/examples/SharedMemory/PhysicsServerCommandProcessor.cpp +++ b/examples/SharedMemory/PhysicsServerCommandProcessor.cpp @@ -1349,9 +1349,18 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm //} } } + btVector3 linkOrigin = mb->getLink(l).m_cachedWorldTransform.getOrigin(); + btQuaternion linkRotation = mb->getLink(l).m_cachedWorldTransform.getRotation(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+0] = linkOrigin.getX(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+1] = linkOrigin.getY(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+2] = linkOrigin.getZ(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+3] = linkRotation.x(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+4] = linkRotation.y(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+5] = linkRotation.z(); + serverCmd.m_sendActualStateArgs.m_linkState[l*7+6] = linkRotation.w(); } - + serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomQ = totalDegreeOfFreedomQ; serverCmd.m_sendActualStateArgs.m_numDegreeOfFreedomU = totalDegreeOfFreedomU; diff --git a/examples/SharedMemory/SharedMemoryCommands.h b/examples/SharedMemory/SharedMemoryCommands.h index 94e0832be..eafe68fe0 100644 --- a/examples/SharedMemory/SharedMemoryCommands.h +++ b/examples/SharedMemory/SharedMemoryCommands.h @@ -31,6 +31,7 @@ #define MAX_NUM_SENSORS 256 #define MAX_URDF_FILENAME_LENGTH 1024 #define MAX_FILENAME_LENGTH MAX_URDF_FILENAME_LENGTH +#define MAX_NUM_LINKS MAX_DEGREE_OF_FREEDOM struct TmpFloat3 { @@ -196,6 +197,7 @@ struct SendActualStateArgs double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM]; double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM]; + double m_linkState[7*MAX_NUM_LINKS]; }; enum EnumSensorTypes diff --git a/examples/SharedMemory/SharedMemoryPublic.h b/examples/SharedMemory/SharedMemoryPublic.h index 315c15779..b5542a01c 100644 --- a/examples/SharedMemory/SharedMemoryPublic.h +++ b/examples/SharedMemory/SharedMemoryPublic.h @@ -105,6 +105,14 @@ struct b3DebugLines const float* m_linesColor;//float red,green,blue times 'm_numDebugLines'. }; +///b3LinkState provides extra information such as the Cartesian world coordinates of the link +///relative to the world reference frame. Orientation is a quaternion x,y,z,w +struct b3LinkState +{ + double m_worldPosition[3]; + double m_worldOrientation[4]; +}; + //todo: discuss and decide about control mode and combinations enum { // POSITION_CONTROL=0,