expose timeout in pybullet/shared memory API

add RobotSimulator, a C++ API similar to pybullet. (work-in-progress, only part of API implemeted)

CMakeLists.txt

@@ -210,7 +210,10 @@ ENDIF()
 OPTION(BUILD_BULLET3 "Set when you want to build Bullet 3" ON)
 
 OPTION(BUILD_PYBULLET "Set when you want to build pybullet (experimental Python bindings for Bullet)" OFF)
-
+
+OPTION(BUILD_ENET "Set when you want to build apps with enet UDP networking support" ON)
+OPTION(BUILD_CLSOCKET "Set when you want to build apps with enet TCP networking support" ON)
+
 
 IF(BUILD_PYBULLET)
 

build3/premake4.lua

@@ -253,6 +253,7 @@ end
 
 	if not _OPTIONS["no-demos"] then
 		include "../examples/ExampleBrowser"
+		include "../examples/RobotSimulator"
 		include "../examples/OpenGLWindow"
 		include "../examples/ThirdPartyLibs/Gwen"
 		include "../examples/HelloWorld"

build_visual_studio_vr_pybullet_double_cmake.bat

@@ -0,0 +1,4 @@
+mkdir build_cmake
+cd build_cmake
+cmake -DBUILD_PYBULLET=ON -DUSE_DOUBLE_PRECISION=ON -DCMAKE_BUILD_TYPE=Release -DPYTHON_INCLUDE_DIR=c:\python-3.5.2\include -DPYTHON_LIBRARY=c:\python-3.5.2\libs\python35.lib -DPYTHON_DEBUG_LIBRARY=c:\python-3.5.2\libs\python35_d.lib -G "Visual Studio 14 2015" ..
+start .

examples/CMakeLists.txt

@@ -1,6 +1,6 @@
 SUBDIRS( HelloWorld BasicDemo )
 IF(BUILD_BULLET3)
-	SUBDIRS( ExampleBrowser SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow )
+	SUBDIRS( ExampleBrowser RobotSimulator SharedMemory ThirdPartyLibs/Gwen ThirdPartyLibs/BussIK ThirdPartyLibs/clsocket OpenGLWindow )
 ENDIF()
 IF(BUILD_PYBULLET)
 	SUBDIRS(pybullet)

examples/RobotSimulator/CMakeLists.txt

@@ -0,0 +1,159 @@
+
+INCLUDE_DIRECTORIES( 
+		${BULLET_PHYSICS_SOURCE_DIR}/src
+		${BULLET_PHYSICS_SOURCE_DIR}/examples
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/enet/include
+		${BULLET_PHYSICS_SOURCE_DIR}/examples/ThirdPartyLibs/clsocket/src
+  )
+
+
+SET(RobotSimulator_SRCS
+		RobotSimulatorMain.cpp
+		b3RobotSimulatorClientAPI.cpp
+		b3RobotSimulatorClientAPI.h	
+		MinitaurSetup.cpp
+		MinitaurSetup.h
+		../../examples/SharedMemory/IKTrajectoryHelper.cpp
+		../../examples/SharedMemory/IKTrajectoryHelper.h
+		../../examples/ExampleBrowser/InProcessExampleBrowser.cpp
+		../../examples/SharedMemory/TinyRendererVisualShapeConverter.cpp
+		../../examples/SharedMemory/TinyRendererVisualShapeConverter.h
+		../../examples/OpenGLWindow/SimpleCamera.cpp
+		../../examples/OpenGLWindow/SimpleCamera.h
+		../../examples/TinyRenderer/geometry.cpp
+		../../examples/TinyRenderer/model.cpp
+		../../examples/TinyRenderer/tgaimage.cpp
+		../../examples/TinyRenderer/our_gl.cpp
+		../../examples/TinyRenderer/TinyRenderer.cpp
+		../../examples/SharedMemory/InProcessMemory.cpp
+		../../examples/SharedMemory/PhysicsClient.cpp
+		../../examples/SharedMemory/PhysicsClient.h
+		../../examples/SharedMemory/PhysicsServer.cpp
+		../../examples/SharedMemory/PhysicsServer.h
+		../../examples/SharedMemory/PhysicsServerExample.cpp
+		../../examples/SharedMemory/SharedMemoryInProcessPhysicsC_API.cpp
+		../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
+		../../examples/SharedMemory/PhysicsServerSharedMemory.h
+		../../examples/SharedMemory/PhysicsDirect.cpp
+		../../examples/SharedMemory/PhysicsDirect.h
+		../../examples/SharedMemory/PhysicsDirectC_API.cpp
+		../../examples/SharedMemory/PhysicsDirectC_API.h
+		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+		../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
+		../../examples/SharedMemory/PhysicsClientSharedMemory.h
+		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
+		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
+		../../examples/SharedMemory/PhysicsClientC_API.cpp
+		../../examples/SharedMemory/PhysicsClientC_API.h
+		../../examples/SharedMemory/Win32SharedMemory.cpp
+		../../examples/SharedMemory/Win32SharedMemory.h
+		../../examples/SharedMemory/PosixSharedMemory.cpp
+		../../examples/SharedMemory/PosixSharedMemory.h
+		../../examples/Utils/b3ResourcePath.cpp
+		../../examples/Utils/b3ResourcePath.h
+		../../examples/Utils/RobotLoggingUtil.cpp
+		../../examples/Utils/RobotLoggingUtil.h
+		../../examples/ThirdPartyLibs/tinyxml/tinystr.cpp
+		../../examples/ThirdPartyLibs/tinyxml/tinyxml.cpp
+		../../examples/ThirdPartyLibs/tinyxml/tinyxmlerror.cpp
+		../../examples/ThirdPartyLibs/tinyxml/tinyxmlparser.cpp
+		../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+		../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+		../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+		../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+		../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+		../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+		../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+		../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+		../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+		../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+		../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+		../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+		../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp 
+		../../examples/MultiThreading/b3PosixThreadSupport.cpp
+		../../examples/MultiThreading/b3Win32ThreadSupport.cpp
+		../../examples/MultiThreading/b3ThreadSupportInterface.cpp
+
+)
+
+IF(BUILD_CLSOCKET)
+ ADD_DEFINITIONS(-DBT_ENABLE_CLSOCKET)
+ENDIF(BUILD_CLSOCKET)
+
+IF(WIN32)
+	LINK_LIBRARIES(
+		${OPENGL_gl_LIBRARY} ${OPENGL_glu_LIBRARY}
+	)
+	IF(BUILD_ENET)
+		ADD_DEFINITIONS(-DWIN32 -DBT_ENABLE_ENET)
+	ENDIF(BUILD_ENET)
+	IF(BUILD_CLSOCKET)
+		ADD_DEFINITIONS(-DWIN32)
+	ENDIF(BUILD_CLSOCKET)
+
+ELSE(WIN32)
+	IF(BUILD_ENET)
+		ADD_DEFINITIONS(-DHAS_SOCKLEN_T -DBT_ENABLE_ENET)
+	ENDIF(BUILD_ENET)
+
+	IF(BUILD_CLSOCKET)	
+		IF(APPLE)
+			ADD_DEFINITIONS(-D_DARWIN)
+		ELSE()
+			ADD_DEFINITIONS(-D_LINUX)
+		ENDIF()
+	ENDIF(BUILD_CLSOCKET)
+ENDIF(WIN32)
+
+
+IF(BUILD_ENET)
+	set(RobotSimulator_SRCS ${RobotSimulator_SRCS} 
+		../../examples/SharedMemory/PhysicsClientUDP.cpp
+		../../examples/SharedMemory/PhysicsClientUDP_C_API.cpp
+		../../examples/SharedMemory/PhysicsClientUDP.h		
+		../../examples/SharedMemory/PhysicsClientUDP_C_API.h
+		../../examples/ThirdPartyLibs/enet/win32.c
+		../../examples/ThirdPartyLibs/enet/unix.c
+		../../examples/ThirdPartyLibs/enet/callbacks.c
+		../../examples/ThirdPartyLibs/enet/compress.c
+		../../examples/ThirdPartyLibs/enet/host.c
+		../../examples/ThirdPartyLibs/enet/list.c
+		../../examples/ThirdPartyLibs/enet/packet.c
+		../../examples/ThirdPartyLibs/enet/peer.c
+		../../examples/ThirdPartyLibs/enet/protocol.c
+	)
+ENDIF(BUILD_ENET)
+
+IF(BUILD_CLSOCKET)
+	set(RobotSimulator_SRCS ${RobotSimulator_SRCS}
+		 ../../examples/SharedMemory/PhysicsClientTCP.cpp
+                 ../../examples/SharedMemory/PhysicsClientTCP.h
+                 ../../examples/SharedMemory/PhysicsClientTCP_C_API.cpp
+                 ../../examples/SharedMemory/PhysicsClientTCP_C_API.h
+		 ../../examples/ThirdPartyLibs/clsocket/src/SimpleSocket.cpp
+		 ../../examples/ThirdPartyLibs/clsocket/src/ActiveSocket.cpp
+		 ../../examples/ThirdPartyLibs/clsocket/src/PassiveSocket.cpp
+	)
+ENDIF()
+
+ADD_EXECUTABLE(App_RobotSimulator  ${RobotSimulator_SRCS})
+
+
+SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES VERSION ${BULLET_VERSION})
+SET_TARGET_PROPERTIES(App_RobotSimulator PROPERTIES DEBUG_POSTFIX "_d")
+
+
+IF(WIN32)
+	IF(BUILD_ENET OR BUILD_CLSOCKET)
+		TARGET_LINK_LIBRARIES(App_RobotSimulator ws2_32 )
+	ENDIF(BUILD_ENET OR BUILD_CLSOCKET)
+ENDIF(WIN32)
+
+
+
+
+TARGET_LINK_LIBRARIES(App_RobotSimulator BulletExampleBrowserLib BulletFileLoader BulletWorldImporter BulletSoftBody BulletDynamics BulletCollision BulletInverseDynamicsUtils BulletInverseDynamics LinearMath OpenGLWindow gwen BussIK Bullet3Common)
+
+

examples/RobotSimulator/MinitaurSetup.cpp

@@ -0,0 +1,96 @@
+#include "MinitaurSetup.h"
+#include "b3RobotSimulatorClientAPI.h"
+
+#include "Bullet3Common/b3HashMap.h"
+
+struct MinitaurSetupInternalData
+{
+	int m_quadrupedUniqueId;
+
+	MinitaurSetupInternalData()
+		:m_quadrupedUniqueId(-1)
+	{
+	}
+
+	b3HashMap<b3HashString, int> m_jointNameToId;
+
+};
+
+
+MinitaurSetup::MinitaurSetup()
+{
+	m_data = new MinitaurSetupInternalData();
+}
+
+MinitaurSetup::~MinitaurSetup()
+{
+	delete m_data;
+}
+
+void MinitaurSetup::resetPose()
+{
+#if 0
+	 def resetPose(self):
+    #right front leg
+    self.disableAllMotors()
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_rightL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_rightL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_rightR_link'],self.quadruped,self.jointNameToId['knee_front_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
+    self.setMotorAngleByName('motor_front_rightR_joint', 1.57)
+    self.setMotorAngleByName('motor_front_rightL_joint',-1.57)
+
+    #left front leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_front_leftL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_front_leftL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_front_leftR_link'],self.quadruped,self.jointNameToId['knee_front_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
+    self.setMotorAngleByName('motor_front_leftR_joint', 1.57)
+    self.setMotorAngleByName('motor_front_leftL_joint',-1.57)
+
+    #right back leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_rightL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_rightL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_rightR_link'],self.quadruped,self.jointNameToId['knee_back_rightL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,0.01,0.2],[0,-0.015,0.2])
+    self.setMotorAngleByName('motor_back_rightR_joint', 1.57)
+    self.setMotorAngleByName('motor_back_rightL_joint',-1.57)
+
+    #left back leg
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftR_joint'],1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftR_link'],-2.2)
+    p.resetJointState(self.quadruped,self.jointNameToId['motor_back_leftL_joint'],-1.57)
+    p.resetJointState(self.quadruped,self.jointNameToId['knee_back_leftL_link'],2.2)
+    p.createConstraint(self.quadruped,self.jointNameToId['knee_back_leftR_link'],self.quadruped,self.jointNameToId['knee_back_leftL_link'],p.JOINT_POINT2POINT,[0,0,0],[0,-0.01,0.2],[0,0.015,0.2])
+    self.setMotorAngleByName('motor_back_leftR_joint', 1.57)
+    self.setMotorAngleByName('motor_back_leftL_joint',-1.57)
+#endif
+}
+
+int MinitaurSetup::setupMinitaur(class b3RobotSimulatorClientAPI* sim, const b3Vector3& startPos, const b3Quaternion& startOrn)
+{
+
+	b3RobotSimulatorLoadUrdfFileArgs args;
+	args.m_startPosition = startPos;
+	args.m_startOrientation = startOrn;
+
+	m_data->m_quadrupedUniqueId = sim->loadURDF("quadruped/quadruped.urdf",args);
+
+	int numJoints = sim->getNumJoints(m_data->m_quadrupedUniqueId);
+	for (int i=0;i<numJoints;i++)
+	{
+		b3JointInfo jointInfo;
+		sim->getJointInfo(m_data->m_quadrupedUniqueId,i,&jointInfo);
+		if (jointInfo.m_jointName)
+		{
+			m_data->m_jointNameToId.insert(jointInfo.m_jointName,i);
+		}
+	}
+
+	resetPose();
+
+	return m_data->m_quadrupedUniqueId;
+}

examples/RobotSimulator/MinitaurSetup.h

@@ -0,0 +1,20 @@
+#ifndef MINITAUR_SIMULATION_SETUP_H
+#define MINITAUR_SIMULATION_SETUP_H
+
+#include "Bullet3Common/b3Vector3.h"
+#include "Bullet3Common/b3Quaternion.h"
+class MinitaurSetup
+{
+	struct MinitaurSetupInternalData* m_data;
+
+public:
+	MinitaurSetup();
+	virtual ~MinitaurSetup();
+
+	int setupMinitaur(class b3RobotSimulatorClientAPI* sim, const class b3Vector3& startPos=b3MakeVector3(0,0,0), const class b3Quaternion& startOrn = b3Quaternion(0,0,0,1));
+
+	void resetPose();
+
+};
+#endif //MINITAUR_SIMULATION_SETUP_H
+

examples/RobotSimulator/RobotSimulatorMain.cpp

@@ -0,0 +1,66 @@
+
+#include "b3RobotSimulatorClientAPI.h"
+#include "../Utils/b3Clock.h"
+
+#include <string.h>
+#include <stdio.h>
+#include <assert.h>
+#define ASSERT_EQ(a,b) assert((a)==(b));
+#include "MinitaurSetup.h"
+int main(int argc, char* argv[])
+{
+	b3RobotSimulatorClientAPI* sim = new b3RobotSimulatorClientAPI();
+
+	sim->connect(eCONNECT_GUI);
+	//Can also use eCONNECT_DIRECT,eCONNECT_SHARED_MEMORY,eCONNECT_UDP,eCONNECT_TCP, for example:
+	//sim->connect(eCONNECT_UDP, "localhost", 1234);
+	sim->configureDebugVisualizer( COV_ENABLE_GUI, 0);
+	sim->configureDebugVisualizer( COV_ENABLE_SHADOWS, 0);//COV_ENABLE_WIREFRAME
+
+	//syncBodies is only needed when connecting to an existing physics server that has already some bodies
+	sim->syncBodies();
+
+	sim->setTimeStep(1./240.);
+
+	sim->setGravity(b3MakeVector3(0,0,-10));
+
+	sim->loadURDF("plane.urdf");
+
+	MinitaurSetup minitaur;
+	int minitaurUid = minitaur.setupMinitaur(sim, b3MakeVector3(0,0,1));
+
+
+	b3RobotSimulatorLoadUrdfFileArgs args;
+	args.m_startPosition.setValue(2,0,1);
+	int r2d2 = sim->loadURDF("r2d2.urdf",args);
+
+	b3RobotSimulatorLoadFileResults sdfResults;
+	if (!sim->loadSDF("two_cubes.sdf",sdfResults))
+	{
+		b3Warning("Can't load SDF!\n");
+	}
+
+	b3Clock clock;
+	double startTime = clock.getTimeInSeconds();
+	double simWallClockSeconds = 20.;
+
+	while (clock.getTimeInSeconds()-startTime < simWallClockSeconds)
+	{
+		sim->stepSimulation();
+	}
+
+	sim->setRealTimeSimulation(true);
+
+	startTime = clock.getTimeInSeconds();
+	while (clock.getTimeInSeconds()-startTime < simWallClockSeconds)
+	{
+		b3Clock::usleep(1000);
+	}
+
+	sim->disconnect();
+	delete sim;
+
+
+}
+
+