migrating from b3RobotSimAPI to b3RobotSimulatorClientAPI (step by step)

allow to run the client/server code in the example browser without this b3RobotSimAPI hack.

examples/RobotSimulator/b3RobotSimulatorClientAPI.cpp

@@ -2,29 +2,32 @@
 
 //#include "SharedMemoryCommands.h"
 
-#include "SharedMemory/PhysicsClientC_API.h"
+#include "../SharedMemory/PhysicsClientC_API.h"
 
 #ifdef BT_ENABLE_ENET
-#include "SharedMemory/PhysicsClientUDP_C_API.h"
+#include "../SharedMemory/PhysicsClientUDP_C_API.h"
 #endif  //PHYSICS_UDP
 
 #ifdef BT_ENABLE_CLSOCKET
-#include "SharedMemory/PhysicsClientTCP_C_API.h"
+#include "../SharedMemory/PhysicsClientTCP_C_API.h"
 #endif  //PHYSICS_TCP
 
-#include "SharedMemory/PhysicsDirectC_API.h"
+#include "../SharedMemory/PhysicsDirectC_API.h"
 
-#include "SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
+#include "../SharedMemory/SharedMemoryInProcessPhysicsC_API.h"
 
-#include "SharedMemory/SharedMemoryPublic.h"
+
+#include "../SharedMemory/SharedMemoryPublic.h"
 #include "Bullet3Common/b3Logging.h"
 
 struct b3RobotSimulatorClientAPI_InternalData
 {
 	b3PhysicsClientHandle m_physicsClientHandle;
+	struct GUIHelperInterface* m_guiHelper;
 
 	b3RobotSimulatorClientAPI_InternalData()
-		: m_physicsClientHandle(0)
+		: m_physicsClientHandle(0),
+		m_guiHelper(0)
 	{
 	}
 };
@@ -39,6 +42,26 @@ b3RobotSimulatorClientAPI::~b3RobotSimulatorClientAPI()
 	delete m_data;
 }
 
+void b3RobotSimulatorClientAPI::setGuiHelper(struct GUIHelperInterface* guiHelper)
+{
+	m_data->m_guiHelper = guiHelper;
+}
+
+void b3RobotSimulatorClientAPI::renderScene()
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return;
+	}
+	if (m_data->m_guiHelper)
+	{
+		b3InProcessRenderSceneInternal(m_data->m_physicsClientHandle);
+	}
+}
+
+
+
 bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, int portOrKey)
 {
 	if (m_data->m_physicsClientHandle)
@@ -55,6 +78,12 @@ bool b3RobotSimulatorClientAPI::connect(int mode, const std::string& hostName, i
 
 	switch (mode)
 	{
+		case eCONNECT_EXISTING_EXAMPLE_BROWSER:
+		{
+			sm = b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(m_data->m_guiHelper);
+			break;
+		}
+
 		case eCONNECT_GUI:
 		{
 			int argc = 0;
@@ -644,8 +673,72 @@ bool b3RobotSimulatorClientAPI::getJointState(int bodyUniqueId, int jointIndex,
 	return false;
 }
 
+bool b3RobotSimulatorClientAPI::getJointStates(int bodyUniqueId, b3JointStates2& state)
+{
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
+
+	b3SharedMemoryCommandHandle command = b3RequestActualStateCommandInit(m_data->m_physicsClientHandle,bodyUniqueId);
+    b3SharedMemoryStatusHandle statusHandle = b3SubmitClientCommandAndWaitStatus(m_data->m_physicsClientHandle, command);
+    
+    if (statusHandle)
+    {
+     //   double rootInertialFrame[7];
+        const double* rootLocalInertialFrame;
+        const double* actualStateQ;
+        const double* actualStateQdot;
+        const double* jointReactionForces;
+        
+        int stat = b3GetStatusActualState(statusHandle,
+                        &state.m_bodyUniqueId,
+                        &state.m_numDegreeOfFreedomQ,
+                        &state.m_numDegreeOfFreedomU,
+                        &rootLocalInertialFrame,
+                        &actualStateQ,
+                        &actualStateQdot,
+                        &jointReactionForces);
+        if (stat)
+        {
+            state.m_actualStateQ.resize(state.m_numDegreeOfFreedomQ);
+            state.m_actualStateQdot.resize(state.m_numDegreeOfFreedomU);
+
+            for (int i=0;i<state.m_numDegreeOfFreedomQ;i++)
+            {
+                state.m_actualStateQ[i] = actualStateQ[i];
+            }
+            for (int i=0;i<state.m_numDegreeOfFreedomU;i++)
+            {
+                state.m_actualStateQdot[i] = actualStateQdot[i];
+            }
+            int numJoints = getNumJoints(bodyUniqueId);
+            state.m_jointReactionForces.resize(6*numJoints);
+            for (int i=0;i<numJoints*6;i++)
+            {
+                state.m_jointReactionForces[i] = jointReactionForces[i];
+            }
+            
+            return true;
+        }
+        //rootInertialFrame,
+          //              &state.m_actualStateQ[0],
+            //            &state.m_actualStateQdot[0],
+              //          &state.m_jointReactionForces[0]);
+        
+     
+    }
+    return false;
+}
+
 bool b3RobotSimulatorClientAPI::resetJointState(int bodyUniqueId, int jointIndex, double targetValue)
 {
+	if (!isConnected())
+	{
+		b3Warning("Not connected");
+		return false;
+	}
 	b3SharedMemoryCommandHandle commandHandle;
 	b3SharedMemoryStatusHandle statusHandle;
 	int numJoints;

examples/RobotSimulator/b3RobotSimulatorClientAPI.h

@@ -124,6 +124,17 @@ struct b3RobotSimulatorInverseKinematicsResults
 	b3AlignedObjectArray<double> m_calculatedJointPositions;
 };
 
+struct b3JointStates2
+{
+    int m_bodyUniqueId;
+    int m_numDegreeOfFreedomQ;
+    int m_numDegreeOfFreedomU;
+    b3Transform m_rootLocalInertialFrame;
+    b3AlignedObjectArray<double> m_actualStateQ;
+    b3AlignedObjectArray<double> m_actualStateQdot;
+    b3AlignedObjectArray<double> m_jointReactionForces;
+};
+
 ///The b3RobotSimulatorClientAPI is pretty much the C++ version of pybullet
 ///as documented in the pybullet Quickstart Guide
 ///https://docs.google.com/document/d/10sXEhzFRSnvFcl3XxNGhnD4N2SedqwdAvK3dsihxVUA
@@ -137,6 +148,11 @@ public:
 	b3RobotSimulatorClientAPI();
 	virtual ~b3RobotSimulatorClientAPI();
 
+	//setGuiHelper is only used when embedded in existing example browser
+	void setGuiHelper(struct GUIHelperInterface* guiHelper);
+	//renderScene is only used when embedded in existing example browser
+	virtual void renderScene();
+
 	bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
 
 	void disconnect();
@@ -177,6 +193,8 @@ public:
 
 	bool getJointState(int bodyUniqueId, int jointIndex, struct b3JointSensorState* state);
 
+	bool getJointStates(int bodyUniqueId, b3JointStates2& state);
+
 	bool resetJointState(int bodyUniqueId, int jointIndex, double targetValue);
 
 	void setJointMotorControl(int bodyUniqueId, int jointIndex, const struct b3RobotSimulatorJointMotorArgs& args);

examples/RoboticsLearning/KukaGraspExample.cpp

@@ -11,8 +11,8 @@
 #include "../SharedMemory/PhysicsServerSharedMemory.h"
 #include "../SharedMemory/PhysicsClientC_API.h"
 #include <string>
+#include "../RobotSimulator/b3RobotSimulatorClientAPI.h"
 
-#include "b3RobotSimAPI.h"
 #include "../Utils/b3Clock.h"
 
 ///quick demo showing the right-handed coordinate system and positive rotations around each axis
@@ -20,7 +20,8 @@ class KukaGraspExample : public CommonExampleInterface
 {
     CommonGraphicsApp* m_app;
 	GUIHelperInterface* m_guiHelper;
-	b3RobotSimAPI m_robotSim;
+	b3RobotSimulatorClientAPI m_robotSim;
+
     int m_kukaIndex;
     
     IKTrajectoryHelper m_ikHelper;
@@ -78,19 +79,18 @@ public:
         
         
         
+        int mode = eCONNECT_EXISTING_EXAMPLE_BROWSER;
+		m_robotSim.setGuiHelper(m_guiHelper);
+		bool connected = m_robotSim.connect(mode);
 			
-		bool connected = m_robotSim.connect(m_guiHelper);
+//			0;//m_robotSim.connect(m_guiHelper);
 		b3Printf("robotSim connected = %d",connected);
 		
 		
         {
-            b3RobotSimLoadFileArgs args("");
+			m_kukaIndex  = m_robotSim.loadURDF("kuka_iiwa/model.urdf");
-            args.m_fileName = "kuka_iiwa/model.urdf";
+			if (m_kukaIndex >=0)
-            args.m_startPosition.setValue(0,0,0);
-            b3RobotSimLoadFileResults results;
-            if (m_robotSim.loadFile(args, results) && results.m_uniqueObjectIds.size()==1)
             {
-                m_kukaIndex = results.m_uniqueObjectIds[0];
                 int numJoints = m_robotSim.getNumJoints(m_kukaIndex);
                 b3Printf("numJoints = %d",numJoints);
 
@@ -113,23 +113,8 @@ public:
                  */
             }
 			
-			if (0)
 			{
-				b3RobotSimLoadFileArgs args("");
+				m_robotSim.loadURDF("plane.urdf");
-				args.m_fileName = "kiva_shelf/model.sdf";
-                args.m_forceOverrideFixedBase = true;
-				args.m_fileType = B3_SDF_FILE;
-                args.m_startOrientation = b3Quaternion(0,0,0,1);
-                b3RobotSimLoadFileResults results;
-                m_robotSim.loadFile(args,results);
-			}
-			{
-				b3RobotSimLoadFileArgs args("");
-				args.m_fileName = "plane.urdf";
-				args.m_startPosition.setValue(0,0,0);
-				args.m_forceOverrideFixedBase = true;
-				b3RobotSimLoadFileResults results;
-				m_robotSim.loadFile(args,results);
 				m_robotSim.setGravity(b3MakeVector3(0,0,0));
 			}
 	
@@ -157,9 +142,7 @@ public:
         {
             double q_current[7]={0,0,0,0,0,0,0};
 
-            double world_position[3]={0,0,0};
+            b3JointStates2 jointStates;
-            double world_orientation[4]={0,0,0,0};
-            b3JointStates jointStates;
             
             if (m_robotSim.getJointStates(m_kukaIndex,jointStates))
             {
@@ -171,9 +154,12 @@ public:
                 }
             }
             // compute body position and orientation
-            m_robotSim.getLinkState(0, 6, world_position, world_orientation);
+			b3LinkState linkState;
-            m_worldPos.setValue(world_position[0], world_position[1], world_position[2]);
+            m_robotSim.getLinkState(0, 6, &linkState);
-            m_worldOri.setValue(world_orientation[0], world_orientation[1], world_orientation[2], world_orientation[3]);
+			
+			m_worldPos.setValue(linkState.m_worldLinkFramePosition[0], linkState.m_worldLinkFramePosition[1], linkState.m_worldLinkFramePosition[2]);
+            m_worldOri.setValue(linkState.m_worldLinkFrameOrientation[0], linkState.m_worldLinkFrameOrientation[1], linkState.m_worldLinkFrameOrientation[2], linkState.m_worldLinkFrameOrientation[3]);
+
 
             b3Vector3DoubleData targetPosDataOut;
             m_targetPos.serializeDouble(targetPosDataOut);
@@ -185,8 +171,8 @@ public:
             m_worldOri.serializeDouble(worldOriDataOut);
 
             
-			b3RobotSimInverseKinematicArgs ikargs;
+			b3RobotSimulatorInverseKinematicArgs ikargs;
-			b3RobotSimInverseKinematicsResults ikresults;
+			b3RobotSimulatorInverseKinematicsResults ikresults;
 			
          
 			ikargs.m_bodyUniqueId = m_kukaIndex;
@@ -247,7 +233,7 @@ public:
                 //copy the IK result to the desired state of the motor/actuator
                 for (int i=0;i<numJoints;i++)
                 {
-                    b3JointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
+                    b3RobotSimulatorJointMotorArgs t(CONTROL_MODE_POSITION_VELOCITY_PD);
                     t.m_targetPosition = ikresults.m_calculatedJointPositions[i];
                     t.m_maxTorqueValue = 100.0;
                     t.m_kp= 1.0;
@@ -257,6 +243,7 @@ public:
 
                 }
             }
+		
         }
         
         

examples/SharedMemory/PhysicsClientSharedMemory.h

@@ -14,7 +14,7 @@ protected:
     void processBodyJointInfo(int bodyUniqueId, const struct SharedMemoryStatus& serverCmd);
     void resetData();
 	void removeCachedBody(int bodyUniqueId);
-
+	virtual void renderSceneInternal() {};
 public:
     PhysicsClientSharedMemory();
     virtual ~PhysicsClientSharedMemory();

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp

@@ -4,6 +4,9 @@
 
 #include "PhysicsClientSharedMemory.h"
 #include"../ExampleBrowser/InProcessExampleBrowser.h"
+#include "PhysicsServerExample.h"
+#include "../CommonInterfaces/CommonExampleInterface.h"
+#include "InProcessMemory.h"
 
 #include "Bullet3Common/b3Logging.h"
 class InProcessPhysicsClientSharedMemoryMainThread : public PhysicsClientSharedMemory
@@ -124,3 +127,100 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
 	return (b3PhysicsClientHandle ) cl;
 }
 
+class InProcessPhysicsClientExistingExampleBrowser : public PhysicsClientSharedMemory
+{
+	CommonExampleInterface*  m_physicsServerExample;
+	SharedMemoryInterface* m_sharedMem;
+	b3Clock m_clock;
+	unsigned long long int m_prevTime;
+public:
+	InProcessPhysicsClientExistingExampleBrowser (struct GUIHelperInterface* guiHelper)
+	{
+		
+
+		m_sharedMem = new InProcessMemory;
+		CommonExampleOptions options(guiHelper);
+		options.m_sharedMem = m_sharedMem;
+			
+		m_physicsServerExample = PhysicsServerCreateFunc(options);
+		m_physicsServerExample ->initPhysics();
+		m_physicsServerExample ->resetCamera();
+		setSharedMemoryInterface(m_sharedMem);
+		m_clock.reset();
+		m_prevTime = m_clock.getTimeMicroseconds();
+
+	}
+	virtual ~InProcessPhysicsClientExistingExampleBrowser()
+	{
+		m_physicsServerExample->exitPhysics();
+		//s_instancingRenderer->removeAllInstances();
+		delete m_physicsServerExample;
+		delete m_sharedMem;
+	}
+
+	 // return non-null if there is a status, nullptr otherwise
+    virtual const struct SharedMemoryStatus* processServerStatus()
+    {
+		//m_physicsServerExample->updateGraphics();
+
+		unsigned long long int curTime = m_clock.getTimeMicroseconds();
+		unsigned long long int dtMicro = curTime - m_prevTime;
+		m_prevTime = curTime;
+
+		double dt = double(dtMicro)/1000000.;
+
+		m_physicsServerExample->stepSimulation(dt);
+
+		#if 0
+		{
+			//if (btIsExampleBrowserMainThreadTerminated(m_data))
+			//{
+			//	PhysicsClientSharedMemory::disconnectSharedMemory();
+			//}
+		}
+			//{	
+	   		//unsigned long int ms = m_clock.getTimeMilliseconds();
+			//if (ms>20)
+			//{ 
+			//	B3_PROFILE("m_clock.reset()");
+		//
+			//	m_clock.reset(); 
+        		//btUpdateInProcessExampleBrowserMainThread(m_data);
+			//}
+		}
+	#endif
+		{
+			b3Clock::usleep(0);
+		}
+		const SharedMemoryStatus* stat = 0;
+
+		{
+			stat = PhysicsClientSharedMemory::processServerStatus();
+		}
+
+		return stat;
+        
+    }
+
+	virtual void renderScene()
+	{
+		m_physicsServerExample->renderScene();
+	}
+
+	
+};
+
+void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle)
+{
+	InProcessPhysicsClientExistingExampleBrowser* cl = (InProcessPhysicsClientExistingExampleBrowser*) clientHandle;
+	cl->renderScene();
+}
+
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper)
+{
+
+	InProcessPhysicsClientExistingExampleBrowser* cl  = new InProcessPhysicsClientExistingExampleBrowser(guiHelper);
+	//InProcessPhysicsClientFromGuiHelper* cl = new InProcessPhysicsClientFromGuiHelper(guiHelper);
+    cl->connect();
+	return (b3PhysicsClientHandle ) cl;
+}

examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.h

@@ -14,8 +14,9 @@ b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnect(int argc, char* a
 
 b3PhysicsClientHandle b3CreateInProcessPhysicsServerAndConnectMainThread(int argc, char* argv[]);
 
+b3PhysicsClientHandle b3CreateInProcessPhysicsServerFromExistingExampleBrowserAndConnect(struct GUIHelperInterface* guiHelper);
     
-    
+void b3InProcessRenderSceneInternal(b3PhysicsClientHandle clientHandle);
     
 #ifdef __cplusplus
 }

examples/SharedMemory/SharedMemoryPublic.h

@@ -503,6 +503,7 @@ enum eCONNECT_METHOD {
   eCONNECT_SHARED_MEMORY = 3,
   eCONNECT_UDP = 4,
   eCONNECT_TCP = 5,
+  eCONNECT_EXISTING_EXAMPLE_BROWSER=6,
 };
 
 enum eURDF_Flags