update RobotSimulator/MinitaurSetup to use data/quadruped/minitaur.urdf

add b3RobotSimulatorClientAPI::getBaseVelocity and resetBaseVelocity add b3Quaternion::getEulerZYX
2017-03-15 17:09:17 -07:00
parent 4db6fa9e29
commit b7b46b12d3
5 changed files with 427 additions and 381 deletions
--- a/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
+++ b/examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp
@@ -1,17 +1,16 @@
 #include "b3RobotSimulatorClientAPI.h"

-
 //#include "SharedMemoryCommands.h"

 #include "SharedMemory/PhysicsClientC_API.h"

 #ifdef BT_ENABLE_ENET
 #include "SharedMemory/PhysicsClientUDP_C_API.h"
-#endif//PHYSICS_UDP
+#endif  //PHYSICS_UDP

 #ifdef BT_ENABLE_CLSOCKET
 #include "SharedMemory/PhysicsClientTCP_C_API.h"
-#endif//PHYSICS_TCP
+#endif  //PHYSICS_TCP

 #include "SharedMemory/PhysicsDirectC_API.h"

@@ -25,7 +24,7 @@ struct b3RobotSimulatorClientAPI_InternalData
 	b3PhysicsClientHandle m_physicsClientHandle;

 	b3RobotSimulatorClientAPI_InternalData()
-		:m_physicsClientHandle(0)
+		: m_physicsClientHandle(0)
 	{
 	}
 };
@@ -35,17 +34,16 @@ b3RobotSimulatorClientAPI::b3RobotSimulatorClientAPI()
 	m_data = new b3RobotSimulatorClientAPI_InternalData();
 }

- b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
+b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
 {
-	 delete m_data;
+	delete m_data;
 }

-
 bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
 {
 	if (m_data->m_physicsClientHandle)
 	{
-		b3Warning ("Already connected, disconnect first.");
+		b3Warning("Already connected, disconnect first.");
 		return false;
 	}
 	b3PhysicsClientHandle sm = 0;
@@ -54,89 +52,88 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 	int tcpPort = 6667;
 	int key = SHARED_MEMORY_KEY;
 	bool connected = false;
-	

-	switch (mode) 
+	switch (mode)
 	{
-		case eCONNECT_GUI: 
+		case eCONNECT_GUI:
 		{
-		int argc = 0;
-		char* argv[1] = {0};
-	#ifdef __APPLE__
-		sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
-	#else
-		sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
-	#endif
-		break;
+			int argc = 0;
+			char* argv[1] = {0};
+#ifdef __APPLE__
+			sm = b3CreateInProcessPhysicsServerAndConnectMainThread(argc, argv);
+#else
+			sm = b3CreateInProcessPhysicsServerAndConnect(argc, argv);
+#endif
+			break;
 		}
-		case eCONNECT_DIRECT: {
-		sm = b3ConnectPhysicsDirect();
-		break;
+		case eCONNECT_DIRECT:
+		{
+			sm = b3ConnectPhysicsDirect();
+			break;
 		}
-		case eCONNECT_SHARED_MEMORY: {
-			if (portOrKey>=0)
+		case eCONNECT_SHARED_MEMORY:
+		{
+			if (portOrKey >= 0)
 			{
 				key = portOrKey;
 			}
-		sm = b3ConnectSharedMemory(key);
-		break;
+			sm = b3ConnectSharedMemory(key);
+			break;
 		}
-		case eCONNECT_UDP: 
-	{
-			if (portOrKey>=0)
+		case eCONNECT_UDP:
+		{
+			if (portOrKey >= 0)
 			{
 				udpPort = portOrKey;
 			}
-	#ifdef BT_ENABLE_ENET
+#ifdef BT_ENABLE_ENET

 			sm = b3ConnectPhysicsUDP(hostName.c_str(), udpPort);
-	#else
- 		b3Warning("UDP is not enabled in this build");
-	#endif //BT_ENABLE_ENET
+#else
+			b3Warning("UDP is not enabled in this build");
+#endif  //BT_ENABLE_ENET

 			break;
-	}
-	case eCONNECT_TCP:
+		}
+		case eCONNECT_TCP:
 		{
-			if (portOrKey>=0)
+			if (portOrKey >= 0)
 			{
 				tcpPort = portOrKey;
 			}
-	#ifdef BT_ENABLE_CLSOCKET
-            
+#ifdef BT_ENABLE_CLSOCKET
+
 			sm = b3ConnectPhysicsTCP(hostName.c_str(), tcpPort);
-	#else
+#else
 			b3Warning("TCP is not enabled in this pybullet build");
-	#endif //BT_ENABLE_CLSOCKET
+#endif  //BT_ENABLE_CLSOCKET
 			break;
 		}
-            
-            

-		default: {
-		b3Warning("connectPhysicsServer unexpected argument");
+		default:
+		{
+			b3Warning("connectPhysicsServer unexpected argument");
 		}
 	};

-  if (sm)
-  {
-	  m_data->m_physicsClientHandle = sm;
-	  if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
-	  {
-		  disconnect();
-		  return false;
-	  }
-	  return true;
-  }
-  return false;
+	if (sm)
+	{
+		m_data->m_physicsClientHandle = sm;
+		if (!b3CanSubmitCommand(m_data->m_physicsClientHandle))
+		{
+			disconnect();
+			return false;
+		}
+		return true;
+	}
+	return false;
 }

 bool b3RobotSimulatorClientAPI::isConnected() const
 {
-	return  (m_data->m_physicsClientHandle!=0);
+	return (m_data->m_physicsClientHandle != 0);
 }

-
 void b3RobotSimulatorClientAPI::disconnect()
 {
 	if (!isConnected())
@@ -157,15 +154,13 @@ void b3RobotSimulatorClientAPI::syncBodies()
 		return;
 	}

-
-	b3SharedMemoryCommandHandle	command;
+	b3SharedMemoryCommandHandle command;
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;

 	command = b3InitSyncBodyInfoCommand(m_data->m_physicsClientHandle);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 	statusType = b3GetStatusType(statusHandle);
-
 }

 void b3RobotSimulatorClientAPI::resetSimulation()
@@ -175,9 +170,9 @@ void b3RobotSimulatorClientAPI::resetSimulation()
 		b3Warning("Not connected");
 		return;
 	}
-    b3SharedMemoryStatusHandle statusHandle;
-    statusHandle = b3SubmitClientCommandAndWaitStatus(
-    m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
+	b3SharedMemoryStatusHandle statusHandle;
+	statusHandle = b3SubmitClientCommandAndWaitStatus(
+		m_data->m_physicsClientHandle, b3InitResetSimulationCommand(m_data->m_physicsClientHandle));
 }

 bool b3RobotSimulatorClientAPI::canSubmitCommand() const
@@ -186,7 +181,7 @@ bool b3RobotSimulatorClientAPI::canSubmitCommand() const
 	{
 		return false;
 	}
-	return (b3CanSubmitCommand(m_data->m_physicsClientHandle));    
+	return (b3CanSubmitCommand(m_data->m_physicsClientHandle) != 0);
 }

 void b3RobotSimulatorClientAPI::stepSimulation()
@@ -200,11 +195,11 @@ void b3RobotSimulatorClientAPI::stepSimulation()
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
 	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
-    {
-        statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
-        statusType = b3GetStatusType(statusHandle);
-        b3Assert(statusType==CMD_STEP_FORWARD_SIMULATION_COMPLETED);
-    }
+	{
+		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitStepSimulationCommand(m_data->m_physicsClientHandle));
+		statusType = b3GetStatusType(statusHandle);
+		b3Assert(statusType == CMD_STEP_FORWARD_SIMULATION_COMPLETED);
+	}
 }

 void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
@@ -215,57 +210,26 @@ void b3RobotSimulatorClientAPI::setGravity(const b3Vector3& gravityAcceleration)
 		return;
 	}

-   b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-    b3SharedMemoryStatusHandle statusHandle;
-	b3PhysicsParamSetGravity(command,  gravityAcceleration[0],gravityAcceleration[1],gravityAcceleration[2]);
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetGravity(command, gravityAcceleration[0], gravityAcceleration[1], gravityAcceleration[2]);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
 }

 b3Quaternion b3RobotSimulatorClientAPI::getQuaternionFromEuler(const b3Vector3& rollPitchYaw)
 {
-    double phi, the, psi;
-    phi = rollPitchYaw[0] / 2.0;
-    the = rollPitchYaw[1] / 2.0;
-    psi = rollPitchYaw[2] / 2.0;
-    double quat[4] = {
-        sin(phi) * cos(the) * cos(psi) - cos(phi) * sin(the) * sin(psi),
-        cos(phi) * sin(the) * cos(psi) + sin(phi) * cos(the) * sin(psi),
-        cos(phi) * cos(the) * sin(psi) - sin(phi) * sin(the) * cos(psi),
-        cos(phi) * cos(the) * cos(psi) + sin(phi) * sin(the) * sin(psi)};
-
-    // normalize the quaternion
-    double len = sqrt(quat[0] * quat[0] + quat[1] * quat[1] +
-                    quat[2] * quat[2] + quat[3] * quat[3]);
-    quat[0] /= len;
-    quat[1] /= len;
-    quat[2] /= len;
-    quat[3] /= len;
-	b3Quaternion q(quat[0],quat[1],quat[2],quat[3]);
+	b3Quaternion q;
+	q.setEulerZYX(rollPitchYaw[2],rollPitchYaw[1],rollPitchYaw[0]);
 	return q;
 }

 b3Vector3 b3RobotSimulatorClientAPI::getEulerFromQuaternion(const b3Quaternion& quat)
 {
-	 double squ;
-  double sqx;
-  double sqy;
-  double sqz;
-    double sarg;
-  
-    b3Vector3 rpy;
-    sqx = quat[0] * quat[0];
-    sqy = quat[1] * quat[1];
-    sqz = quat[2] * quat[2];
-    squ = quat[3] * quat[3];
-    rpy[0] = atan2(2 * (quat[1] * quat[2] + quat[3] * quat[0]),
-                   squ - sqx - sqy + sqz);
-    sarg = -2 * (quat[0] * quat[2] - quat[3] * quat[1]);
-    rpy[1] = sarg <= -1.0 ? -0.5 * 3.141592538
-                          : (sarg >= 1.0 ? 0.5 * 3.141592538 : asin(sarg));
-    rpy[2] = atan2(2 * (quat[0] * quat[1] + quat[3] * quat[2]),
-                   squ + sqx - sqy - sqz);
-	return rpy;
+	b3Scalar roll,pitch,yaw;
+	quat.getEulerZYX(yaw,pitch,roll);
+	b3Vector3 rpy2 = b3MakeVector3(roll,pitch,yaw);
+	return rpy2;
 }

 int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struct b3RobotSimulatorLoadUrdfFileArgs& args)
@@ -278,31 +242,26 @@ int b3RobotSimulatorClientAPI::loadURDF(const std::string& fileName, const struc
 		return robotUniqueId;
 	}

-	
-
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
 	b3SharedMemoryCommandHandle command = b3LoadUrdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-			
+
 	//setting the initial position, orientation and other arguments are optional
-            
+
 	b3LoadUrdfCommandSetStartPosition(command, args.m_startPosition[0],
-													args.m_startPosition[1],
-													args.m_startPosition[2]);
-	b3LoadUrdfCommandSetStartOrientation(command,args.m_startOrientation[0]
-											,args.m_startOrientation[1]
-											,args.m_startOrientation[2]
-											,args.m_startOrientation[3]);
+									  args.m_startPosition[1],
+									  args.m_startPosition[2]);
+	b3LoadUrdfCommandSetStartOrientation(command, args.m_startOrientation[0], args.m_startOrientation[1], args.m_startOrientation[2], args.m_startOrientation[3]);
 	if (args.m_forceOverrideFixedBase)
 	{
-		b3LoadUrdfCommandSetUseFixedBase(command,true);
+		b3LoadUrdfCommandSetUseFixedBase(command, true);
 	}
 	b3LoadUrdfCommandSetUseMultiBody(command, args.m_useMultiBody);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 	statusType = b3GetStatusType(statusHandle);

-	b3Assert(statusType==CMD_URDF_LOADING_COMPLETED);
-	if (statusType==CMD_URDF_LOADING_COMPLETED)
+	b3Assert(statusType == CMD_URDF_LOADING_COMPLETED);
+	if (statusType == CMD_URDF_LOADING_COMPLETED)
 	{
 		robotUniqueId = b3GetStatusBodyIndex(statusHandle);
 	}
@@ -329,12 +288,12 @@ bool b3RobotSimulatorClientAPI::loadMJCF(const std::string& fileName, b3RobotSim
 		b3Warning("Couldn't load .mjcf file.");
 		return false;
 	}
-	int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
+	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
 	if (numBodies)
 	{
 		results.m_uniqueObjectIds.resize(numBodies);
 		int numBodies;
-		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
+		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
 	}

 	return true;
@@ -359,12 +318,12 @@ bool b3RobotSimulatorClientAPI::loadBullet(const std::string& fileName, b3RobotS
 	{
 		return false;
 	}
-	int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
+	int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
 	if (numBodies)
 	{
 		results.m_uniqueObjectIds.resize(numBodies);
 		int numBodies;
-		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
+		numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
 	}

 	return true;
@@ -383,19 +342,18 @@ bool b3RobotSimulatorClientAPI::loadSDF(const std::string& fileName, b3RobotSimu
 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
 	b3SharedMemoryCommandHandle command = b3LoadSdfCommandInit(m_data->m_physicsClientHandle, fileName.c_str());
-    b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
+	b3LoadSdfCommandSetUseMultiBody(command, args.m_useMultiBody);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 	statusType = b3GetStatusType(statusHandle);
 	b3Assert(statusType == CMD_SDF_LOADING_COMPLETED);
 	if (statusType == CMD_SDF_LOADING_COMPLETED)
 	{
-		int numBodies = b3GetStatusBodyIndices(statusHandle, 0,0);
+		int numBodies = b3GetStatusBodyIndices(statusHandle, 0, 0);
 		if (numBodies)
 		{
 			results.m_uniqueObjectIds.resize(numBodies);
 			int numBodies;
-			numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0],results.m_uniqueObjectIds.size());
-
+			numBodies = b3GetStatusBodyIndices(statusHandle, &results.m_uniqueObjectIds[0], results.m_uniqueObjectIds.size());
 		}
 		statusOk = true;
 	}
@@ -411,7 +369,7 @@ bool b3RobotSimulatorClientAPI::getBodyInfo(int bodyUniqueId, struct b3BodyInfo*
 		return false;
 	}

-	int result = b3GetBodyInfo(m_data->m_physicsClientHandle,  bodyUniqueId, bodyInfo);
+	int result = b3GetBodyInfo(m_data->m_physicsClientHandle, bodyUniqueId, bodyInfo);
 	return (result != 0);
 }

@@ -431,7 +389,8 @@ bool b3RobotSimulatorClientAPI::getBasePositionAndOrientation(int bodyUniqueId,
 	const int status_type = b3GetStatusType(status_handle);
 	const double* actualStateQ;

-	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED) {
+	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	{
 		return false;
 	}

@@ -466,14 +425,72 @@ bool b3RobotSimulatorClientAPI::resetBasePositionAndOrientation(int bodyUniqueId

 	b3CreatePoseCommandSetBasePosition(commandHandle, basePosition[0], basePosition[1], basePosition[2]);
 	b3CreatePoseCommandSetBaseOrientation(commandHandle, baseOrientation[0], baseOrientation[1],
-										baseOrientation[2], baseOrientation[3]);
+										  baseOrientation[2], baseOrientation[3]);

 	b3SharedMemoryStatusHandle statusHandle;
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-  
+
 	return true;
 }

+bool b3RobotSimulatorClientAPI::getBaseVelocity(int bodyUniqueId, b3Vector3& baseLinearVelocity, b3Vector3& baseAngularVelocity) const
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	const int status_type = b3GetStatusType(statusHandle);
+	const double* actualStateQdot;
+
+	if (status_type != CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	{
+		return false;
+	}
+
+	b3GetStatusActualState(statusHandle, 0 /* body_unique_id */,
+						   0 /* num_degree_of_freedom_q */, 0 /* num_degree_of_freedom_u */,
+						   0 /*root_local_inertial_frame*/, 0,
+						   &actualStateQdot, 0 /* joint_reaction_forces */);
+
+	baseLinearVelocity[0] = actualStateQdot[0];
+	baseLinearVelocity[1] = actualStateQdot[1];
+	baseLinearVelocity[2] = actualStateQdot[2];
+
+	baseAngularVelocity[0] = actualStateQdot[3];
+	baseAngularVelocity[1] = actualStateQdot[4];
+	baseAngularVelocity[2] = actualStateQdot[5];
+	return true;
+}
+
+bool b3RobotSimulatorClientAPI::resetBaseVelocity(int bodyUniqueId, const b3Vector3& linearVelocity, const b3Vector3& angularVelocity) const
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+
+	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+
+	b3Vector3DoubleData linVelDouble;
+	linearVelocity.serializeDouble(linVelDouble);
+	b3CreatePoseCommandSetBaseLinearVelocity(commandHandle, linVelDouble.m_floats);
+
+	b3Vector3DoubleData angVelDouble;
+	angularVelocity.serializeDouble(angVelDouble);
+	b3CreatePoseCommandSetBaseAngularVelocity(commandHandle, angVelDouble.m_floats);
+
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	return true;
+}

 void b3RobotSimulatorClientAPI::setRealTimeSimulation(bool enableRealTimeSimulation)
 {
@@ -500,10 +517,9 @@ int b3RobotSimulatorClientAPI::getNumJoints(int bodyUniqueId) const
 		b3Warning("Not connected");
 		return false;
 	}
-	return b3GetNumJoints(m_data->m_physicsClientHandle,bodyUniqueId);
+	return b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
 }

-
 bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b3JointInfo* jointInfo)
 {
 	if (!isConnected())
@@ -511,7 +527,7 @@ bool b3RobotSimulatorClientAPI::getJointInfo(int bodyUniqueId, int jointIndex, b
 		b3Warning("Not connected");
 		return false;
 	}
-	return (b3GetJointInfo(m_data->m_physicsClientHandle,bodyUniqueId, jointIndex,jointInfo)!=0);
+	return (b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, jointInfo) != 0);
 }

 void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parentJointIndex, int childBodyIndex, int childJointIndex, b3JointInfo* jointInfo)
@@ -521,55 +537,55 @@ void b3RobotSimulatorClientAPI::createConstraint(int parentBodyIndex, int parent
 		b3Warning("Not connected");
 		return;
 	}
-    b3SharedMemoryStatusHandle statusHandle;
-    b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
-    if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
-    {
-        statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
-    }
+	b3SharedMemoryStatusHandle statusHandle;
+	b3Assert(b3CanSubmitCommand(m_data->m_physicsClientHandle));
+	if (b3CanSubmitCommand(m_data->m_physicsClientHandle))
+	{
+		statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, b3InitCreateUserConstraintCommand(m_data->m_physicsClientHandle, parentBodyIndex, parentJointIndex, childBodyIndex, childJointIndex, jointInfo));
+	}
 }

-bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState *state)
+bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state)
 {
 	if (!isConnected())
 	{
 		b3Warning("Not connected");
 		return false;
 	}
-    b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
-    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    int statusType = b3GetStatusType(statusHandle);
-	if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED) 
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	int statusType = b3GetStatusType(statusHandle);
+	if (statusType == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
 	{
-	  if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
-	  {
-		  return true;
-	  }
+		if (b3GetJointState(m_data->m_physicsClientHandle, statusHandle, jointIndex, state))
+		{
+			return true;
+		}
 	}
-    return false;
+	return false;
 }

 bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
 {
-    b3SharedMemoryCommandHandle commandHandle;
-    b3SharedMemoryStatusHandle statusHandle;
-    int numJoints;
+	b3SharedMemoryCommandHandle commandHandle;
+	b3SharedMemoryStatusHandle statusHandle;
+	int numJoints;

-    numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
-    if ((jointIndex >= numJoints) || (jointIndex < 0)) {
-    return false;
-    }
+	numJoints = b3GetNumJoints(m_data->m_physicsClientHandle, bodyUniqueId);
+	if ((jointIndex >= numJoints) || (jointIndex < 0))
+	{
+		return false;
+	}

-    commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	commandHandle = b3CreatePoseCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);

-    b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
-                                        targetValue);
+	b3CreatePoseCommandSetJointPosition(m_data->m_physicsClientHandle, commandHandle, jointIndex,
+										targetValue);

-    statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
 	return false;
 }

-
 void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int jointIndex, const b3RobotSimulatorJointMotorArgs& args)
 {
 	if (!isConnected())
@@ -582,39 +598,39 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
 	{
 		case CONTROL_MODE_VELOCITY:
 		{
-			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_VELOCITY);
 			b3JointInfo jointInfo;
 			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
 			int uIndex = jointInfo.m_uIndex;
-            b3JointControlSetKd(command,uIndex,args.m_kd);
-			b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
-			b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
+			b3JointControlSetKd(command, uIndex, args.m_kd);
+			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
+			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
 			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 			break;
 		}
 		case CONTROL_MODE_POSITION_VELOCITY_PD:
 		{
-			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_POSITION_VELOCITY_PD);
 			b3JointInfo jointInfo;
 			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
 			int uIndex = jointInfo.m_uIndex;
 			int qIndex = jointInfo.m_qIndex;

-			b3JointControlSetDesiredPosition(command,qIndex,args.m_targetPosition);
-			b3JointControlSetKp(command,uIndex,args.m_kp);
-			b3JointControlSetDesiredVelocity(command,uIndex,args.m_targetVelocity);
-			b3JointControlSetKd(command,uIndex,args.m_kd);
-			b3JointControlSetMaximumForce(command,uIndex,args.m_maxTorqueValue);
+			b3JointControlSetDesiredPosition(command, qIndex, args.m_targetPosition);
+			b3JointControlSetKp(command, uIndex, args.m_kp);
+			b3JointControlSetDesiredVelocity(command, uIndex, args.m_targetVelocity);
+			b3JointControlSetKd(command, uIndex, args.m_kd);
+			b3JointControlSetMaximumForce(command, uIndex, args.m_maxTorqueValue);
 			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 			break;
 		}
 		case CONTROL_MODE_TORQUE:
 		{
-			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2( m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
+			b3SharedMemoryCommandHandle command = b3JointControlCommandInit2(m_data->m_physicsClientHandle, bodyUniqueId, CONTROL_MODE_TORQUE);
 			b3JointInfo jointInfo;
 			b3GetJointInfo(m_data->m_physicsClientHandle, bodyUniqueId, jointIndex, &jointInfo);
 			int uIndex = jointInfo.m_uIndex;
-			b3JointControlSetDesiredForceTorque(command,uIndex,args.m_maxTorqueValue);
+			b3JointControlSetDesiredForceTorque(command, uIndex, args.m_maxTorqueValue);
 			statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
 			break;
 		}
@@ -625,7 +641,6 @@ void b3RobotSimulatorClientAPI::setJointMotorControl(int bodyUniqueId, int joint
 	}
 }

-
 void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
 {
 	if (!isConnected())
@@ -633,12 +648,11 @@ void b3RobotSimulatorClientAPI::setNumSolverIterations(int numIterations)
 		b3Warning("Not connected");
 		return;
 	}
-    b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-    b3SharedMemoryStatusHandle statusHandle;
-    b3PhysicsParamSetNumSolverIterations(command, numIterations);
-    statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
-
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetNumSolverIterations(command, numIterations);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
 }

 void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
@@ -654,10 +668,8 @@ void b3RobotSimulatorClientAPI::setTimeStep(double timeStepInSeconds)
 	int ret;
 	ret = b3PhysicsParamSetTimeStep(command, timeStepInSeconds);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-	
 }

-
 void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
 {
 	if (!isConnected())
@@ -665,14 +677,13 @@ void b3RobotSimulatorClientAPI::setNumSimulationSubSteps(int numSubSteps)
 		b3Warning("Not connected");
 		return;
 	}
-    b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
-    b3SharedMemoryStatusHandle statusHandle;
-    b3PhysicsParamSetNumSubSteps(command, numSubSteps);
-    statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    b3Assert(b3GetStatusType(statusHandle)==CMD_CLIENT_COMMAND_COMPLETED);
+	b3SharedMemoryCommandHandle command = b3InitPhysicsParamCommand(m_data->m_physicsClientHandle);
+	b3SharedMemoryStatusHandle statusHandle;
+	b3PhysicsParamSetNumSubSteps(command, numSubSteps);
+	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+	b3Assert(b3GetStatusType(statusHandle) == CMD_CLIENT_COMMAND_COMPLETED);
 }

-
 bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotSimulatorInverseKinematicArgs& args, struct b3RobotSimulatorInverseKinematicsResults& results)
 {
 	if (!isConnected())
@@ -680,55 +691,52 @@ bool b3RobotSimulatorClientAPI::calculateInverseKinematics(const struct b3RobotS
 		b3Warning("Not connected");
 		return false;
 	}
-	b3Assert(args.m_endEffectorLinkIndex>=0);
-	b3Assert(args.m_bodyUniqueId>=0);
-	
+	b3Assert(args.m_endEffectorLinkIndex >= 0);
+	b3Assert(args.m_bodyUniqueId >= 0);

-	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle,args.m_bodyUniqueId);
+	b3SharedMemoryCommandHandle command = b3CalculateInverseKinematicsCommandInit(m_data->m_physicsClientHandle, args.m_bodyUniqueId);
 	if ((args.m_flags & B3_HAS_IK_TARGET_ORIENTATION) && (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY))
-    {
-        b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
-    } else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
 	{
-		b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition,args.m_endEffectorTargetOrientation);
-	} else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
-    {
-        b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
-    } else
-	{
-		b3CalculateInverseKinematicsAddTargetPurePosition(command,args.m_endEffectorLinkIndex,args.m_endEffectorTargetPosition);
+		b3CalculateInverseKinematicsPosOrnWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
+	}
+	else if (args.m_flags & B3_HAS_IK_TARGET_ORIENTATION)
+	{
+		b3CalculateInverseKinematicsAddTargetPositionWithOrientation(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, args.m_endEffectorTargetOrientation);
+	}
+	else if (args.m_flags & B3_HAS_NULL_SPACE_VELOCITY)
+	{
+		b3CalculateInverseKinematicsPosWithNullSpaceVel(command, args.m_numDegreeOfFreedom, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition, &args.m_lowerLimits[0], &args.m_upperLimits[0], &args.m_jointRanges[0], &args.m_restPoses[0]);
+	}
+	else
+	{
+		b3CalculateInverseKinematicsAddTargetPurePosition(command, args.m_endEffectorLinkIndex, args.m_endEffectorTargetPosition);
 	}
-    
-    if (args.m_flags & B3_HAS_JOINT_DAMPING)
-    {
-        b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
-    }

-    b3SharedMemoryStatusHandle statusHandle;
+	if (args.m_flags & B3_HAS_JOINT_DAMPING)
+	{
+		b3CalculateInverseKinematicsSetJointDamping(command, args.m_numDegreeOfFreedom, &args.m_jointDamping[0]);
+	}
+
+	b3SharedMemoryStatusHandle statusHandle;
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);

-	
+	int numPos = 0;

-    int numPos=0;
-    
 	bool result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
-		&results.m_bodyUniqueId,
-		&numPos,
-		0)!=0;
-    if (result && numPos)
-    {
-        results.m_calculatedJointPositions.resize(numPos);
-        result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
-                                                                 &results.m_bodyUniqueId,
-                                                                 &numPos,
-                                &results.m_calculatedJointPositions[0])!=0;
-        
-    }
+															 &results.m_bodyUniqueId,
+															 &numPos,
+															 0) != 0;
+	if (result && numPos)
+	{
+		results.m_calculatedJointPositions.resize(numPos);
+		result = b3GetStatusInverseKinematicsJointPositions(statusHandle,
+															&results.m_bodyUniqueId,
+															&numPos,
+															&results.m_calculatedJointPositions[0]) != 0;
+	}
 	return result;
 }

-
-
 bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex, const double* localPosition, const double* jointPositions, const double* jointVelocities, const double* jointAccelerations, double* linearJacobian, double* angularJacobian)
 {
 	if (!isConnected())
@@ -736,12 +744,12 @@ bool b3RobotSimulatorClientAPI::getBodyJacobian(int bodyUniqueId, int linkIndex,
 		b3Warning("Not connected");
 		return false;
 	}
-    b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
-    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    
-    if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
-    {
-        b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
+	b3SharedMemoryCommandHandle command = b3CalculateJacobianCommandInit(m_data->m_physicsClientHandle, bodyUniqueId, linkIndex, localPosition, jointPositions, jointVelocities, jointAccelerations);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	if (b3GetStatusType(statusHandle) == CMD_CALCULATED_JACOBIAN_COMPLETED)
+	{
+		b3GetStatusJacobian(statusHandle, linearJacobian, angularJacobian);
 		return true;
 	}
 	return false;
@@ -754,14 +762,14 @@ bool b3RobotSimulatorClientAPI::getLinkState(int bodyUniqueId, int linkIndex, b3
 		b3Warning("Not connected");
 		return false;
 	}
-    b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
-    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
-    
-    if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
-    {
-        b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle, bodyUniqueId);
+	b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+
+	if (b3GetStatusType(statusHandle) == CMD_ACTUAL_STATE_UPDATE_COMPLETED)
+	{
+		b3GetLinkState(m_data->m_physicsClientHandle, statusHandle, linkIndex, linkState);
 		return true;
-    }
+	}
 	return false;
 }

@@ -787,9 +795,9 @@ void b3RobotSimulatorClientAPI::getVREvents(b3VREventsData* vrEventsData)
 		return;
 	}

-	b3SharedMemoryCommandHandle	commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
+	b3SharedMemoryCommandHandle commandHandle = b3RequestVREventsCommandInit(m_data->m_physicsClientHandle);
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);	
+	b3GetVREventsData(m_data->m_physicsClientHandle, vrEventsData);
 }

 void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboardEventsData)
@@ -802,32 +810,35 @@ void b3RobotSimulatorClientAPI::getKeyboardEvents(b3KeyboardEventsData* keyboard
 		return;
 	}

-	b3SharedMemoryCommandHandle	commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
+	b3SharedMemoryCommandHandle commandHandle = b3RequestKeyboardEventsCommandInit(m_data->m_physicsClientHandle);
 	b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
-	b3GetKeyboardEventsData(m_data->m_physicsClientHandle,keyboardEventsData);
-
+	b3GetKeyboardEventsData(m_data->m_physicsClientHandle, keyboardEventsData);
 }

-
-int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds)
+int b3RobotSimulatorClientAPI::startStateLogging(b3StateLoggingType loggingType, const std::string& fileName, const b3AlignedObjectArray<int>& objectUniqueIds, int maxLogDof)
 {
 	int loggingUniqueId = -1;
 	b3SharedMemoryCommandHandle commandHandle;
 	commandHandle = b3StateLoggingCommandInit(m_data->m_physicsClientHandle);
-		
-	b3StateLoggingStart(commandHandle,loggingType,fileName.c_str());

-	for ( int i=0;i<objectUniqueIds.size();i++)
+	b3StateLoggingStart(commandHandle, loggingType, fileName.c_str());
+
+	for (int i = 0; i < objectUniqueIds.size(); i++)
 	{
 		int objectUid = objectUniqueIds[i];
-		b3StateLoggingAddLoggingObjectUniqueId(commandHandle,objectUid);
+		b3StateLoggingAddLoggingObjectUniqueId(commandHandle, objectUid);
+	}
+
+	if (maxLogDof > 0)
+	{
+		b3StateLoggingSetMaxLogDof(commandHandle, maxLogDof);
 	}

 	b3SharedMemoryStatusHandle statusHandle;
 	int statusType;
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
 	statusType = b3GetStatusType(statusHandle);
-	if (statusType==CMD_STATE_LOGGING_START_COMPLETED)
+	if (statusType == CMD_STATE_LOGGING_START_COMPLETED)
 	{
 		loggingUniqueId = b3GetStatusLoggingUniqueId(statusHandle);
 	}
@@ -842,4 +853,3 @@ void b3RobotSimulatorClientAPI::stopStateLogging(int stateLoggerUniqueId)
 	b3StateLoggingStop(commandHandle, stateLoggerUniqueId);
 	statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, commandHandle);
 }
-