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

@@ -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)
 {

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);

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");

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");

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -5205,6 +5205,17 @@ bool PhysicsServerCommandProcessor::processCommand(const struct SharedMemoryComm
 							btAlignedObjectArray<btVector3> omega;
 							btAlignedObjectArray<btVector3> linVel;
 							
+							bool computeForwardKinematics = ((clientCmd.m_updateFlags & ACTUAL_STATE_COMPUTE_FORWARD_KINEMATICS)!=0);
+							if (computeForwardKinematics)
+							{
+								B3_PROFILE("compForwardKinematics");
+								btAlignedObjectArray<btQuaternion> world_to_local;
+								btAlignedObjectArray<btVector3> 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)
 							{

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

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};
+	static char* kwlist[] = {"bodyUniqueId", "linkIndex", "computeLinkVelocity", "computeForwardKinematics", "physicsClientId", NULL};
-	if (!PyArg_ParseTupleAndKeywords(args, keywds, "ii|ii", kwlist, &bodyUniqueId, &linkIndex,&computeLinkVelocity, &physicsClientId))
+	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);