expose the local inertial frame for each link in the shared memory API

struct b3LinkState
{
    double m_worldPosition[3];//this is the inertial frame
    double m_worldOrientation[4];

    double m_localInertialPosition[3];//this is the local frame from inertial to link frame
    double m_localInertialOrientation[4];
};

  const btTransform link_frame_world =
               inertial_frame_world * m_local_inertial_frame->inverse();

examples/SharedMemory/PhysicsClientC_API.cpp

@@ -233,10 +233,12 @@ void b3GetLinkState(b3PhysicsClientHandle physClient, b3SharedMemoryStatusHandle
     for (int i = 0; i < 3; ++i) 
     {
       state->m_worldPosition[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + i];
+      state->m_localInertialPosition[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + i];
     }
     for (int i = 0; i < 4; ++i) 
     {
       state->m_worldOrientation[i] = status->m_sendActualStateArgs.m_linkState[7 * linkIndex + 3 + i];
+      state->m_localInertialOrientation[i] = status->m_sendActualStateArgs.m_linkLocalInertialFrames[7 * linkIndex + 3 + i];
     }
   }
 }

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -82,6 +82,7 @@ struct InteralBodyData
 	int m_testData;
 
 	btTransform m_rootLocalInertialFrame;
+	btAlignedObjectArray<btTransform> m_linkLocalInertialFrames;
 
 	InteralBodyData()
 		:m_multiBody(0),
@@ -763,11 +764,18 @@ bool PhysicsServerCommandProcessor::loadUrdf(const char* fileName, const btVecto
 			    //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
 				util->m_memSerializer->m_skipPointers.insert(mb->getBaseCollider(),0);
 
+                bodyHandle->m_linkLocalInertialFrames.reserve(mb->getNumLinks());
 			    for (int i=0;i<mb->getNumLinks();i++)
                 {
 					//disable serialization of the collision objects
                    util->m_memSerializer->m_skipPointers.insert(mb->getLink(i).m_collider,0);
 				   int urdfLinkIndex = creation.m_mb2urdfLink[i];
+				   btScalar mass;
+                   btVector3 localInertiaDiagonal(0,0,0);
+                   btTransform localInertialFrame;
+				   u2b.getMassAndInertia(urdfLinkIndex, mass,localInertiaDiagonal,localInertialFrame);
+				   bodyHandle->m_linkLocalInertialFrames.push_back(localInertialFrame);
+
 				   std::string* linkName = new std::string(u2b.getLinkName(urdfLinkIndex).c_str());
 				   m_data->m_strings.push_back(linkName);
 				   util->m_memSerializer->registerNameForPointer(linkName->c_str(),linkName->c_str());
@@ -1349,8 +1357,12 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                                         //}
                                     }
                                 }
+                btVector3 linkLocalInertialOrigin = body->m_linkLocalInertialFrames[l].getOrigin();
+                btQuaternion linkLocalInertialRotation = body->m_linkLocalInertialFrames[l].getRotation();
+                
                 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();
@@ -1358,6 +1370,16 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
                 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_linkLocalInertialFrames[l*7+0] = linkLocalInertialOrigin.getX();
+                serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+1] = linkLocalInertialOrigin.getY();
+                serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+2] = linkLocalInertialOrigin.getZ();
+                
+                serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+3] = linkLocalInertialRotation.x();
+                serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+4] = linkLocalInertialRotation.y();
+                serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+5] = linkLocalInertialRotation.z();
+                serverCmd.m_sendActualStateArgs.m_linkLocalInertialFrames[l*7+6] = linkLocalInertialRotation.w();
+                
                             }
 
  

examples/SharedMemory/SharedMemoryCommands.h

@@ -197,7 +197,9 @@ struct SendActualStateArgs
     double m_jointReactionForces[6*MAX_DEGREE_OF_FREEDOM];
 
     double m_jointMotorForce[MAX_DEGREE_OF_FREEDOM];
+    
     double m_linkState[7*MAX_NUM_LINKS];
+    double m_linkLocalInertialFrames[7*MAX_NUM_LINKS];
 };
 
 enum EnumSensorTypes

examples/SharedMemory/SharedMemoryPublic.h

@@ -105,12 +105,18 @@ 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
+///b3LinkState provides extra information such as the Cartesian world coordinates
-///relative to the world reference frame. Orientation is a quaternion x,y,z,w
+///center of mass (COM) of the link, relative to the world reference frame.
+///Orientation is a quaternion x,y,z,w
+///Note: to compute the URDF link frame (which equals the joint frame at joint position 0)
+///use URDF link frame = link COM frame * inertiaFrame.inverse()
 struct b3LinkState
 {
     double m_worldPosition[3];
     double m_worldOrientation[4];
+
+    double m_localInertialPosition[3];
+    double m_localInertialOrientation[4];
 };
 
 //todo: discuss and decide about control mode and combinations