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,