enable pdControlPlugin by default (requires pdControlPlugin.cpp and b…

…3RobotSimulatorClientAPI_NoDirect.cpp)

add pdControl.py example, make pdControlPlugin functional
reduce memory usage
fix examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf, fixes issue bulletphysics#1704

Extras/BulletRobotics/CMakeLists.txt

@@ -9,6 +9,8 @@ INCLUDE_DIRECTORIES(
 )
 
 SET(BulletRobotics_SRCS
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h
 		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

examples/ExampleBrowser/CMakeLists.txt

@@ -145,6 +145,8 @@ SET(BulletExampleBrowser_SRCS
 	../TinyRenderer/tgaimage.cpp
 	../TinyRenderer/our_gl.cpp
 	../TinyRenderer/TinyRenderer.cpp
+	../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
+	../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
 	../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
 	../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
 	../SharedMemory/IKTrajectoryHelper.cpp

examples/ExampleBrowser/premake4.lua

@@ -117,6 +117,8 @@ project "App_BulletExampleBrowser"
 		"../SharedMemory/b3PluginManager.cpp",		
 		"../SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
 		"../SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
+		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
+		"../SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 		"../SharedMemory/SharedMemoryCommands.h",
 		"../SharedMemory/SharedMemoryPublic.h",
 		"../SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",

examples/Importers/ImportURDFDemo/URDF2Bullet.cpp

@@ -595,7 +595,7 @@ void ConvertURDF2BulletInternal(
 					}
                 } else
                 {
-					if (cache.m_bulletMultiBody->getBaseMass()==0)
+					if (cache.m_bulletMultiBody->getBaseMass()==0 && cache.m_bulletMultiBody->getNumLinks()==0)
 					{
 						//col->setCollisionFlags(btCollisionObject::CF_KINEMATIC_OBJECT);
 						col->setCollisionFlags(btCollisionObject::CF_STATIC_OBJECT);

examples/RobotSimulator/premake4.lua

@@ -1,6 +1,9 @@
 
 myfiles = 
 {
+
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.cpp",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoGUI.h",
 			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",

examples/SharedMemory/CMakeLists.txt

@@ -1,5 +1,9 @@
 
 SET(SharedMemory_SRCS
+	plugins/pdControlPlugin/pdControlPlugin.cpp
+	plugins/pdControlPlugin/pdControlPlugin.h
+	b3RobotSimulatorClientAPI_NoDirect.cpp
+	b3RobotSimulatorClientAPI_NoDirect.h
 	IKTrajectoryHelper.cpp
 	IKTrajectoryHelper.h
 	PhysicsClient.cpp

examples/SharedMemory/PhysicsServerCommandProcessor.cpp

@@ -45,9 +45,9 @@
 #include "../Extras/Serialize/BulletFileLoader/btBulletFile.h"
 #include "BulletCollision/NarrowPhaseCollision/btRaycastCallback.h"
 
-#ifdef STATIC_PD_CONTROL_PLUGIN
+#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
 #include "plugins/pdControlPlugin/pdControlPlugin.h"
-#endif //STATIC_PD_CONTROL_PLUGIN
+#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
 
 #ifdef STATIC_LINK_VR_PLUGIN
 #include "plugins/vrSyncPlugin/vrSyncPlugin.h"
@@ -105,7 +105,6 @@ struct UrdfLinkNameMapUtil
 	}
 	virtual ~UrdfLinkNameMapUtil()
 	{
-		delete m_memSerializer;
 	}
 };
 
@@ -1599,7 +1598,7 @@ struct PhysicsServerCommandProcessorInternalData
 
 
 	btAlignedObjectArray<btMultiBodyWorldImporter*> m_worldImporters;
-	btAlignedObjectArray<UrdfLinkNameMapUtil*> m_urdfLinkNameMapper;
+
 	btAlignedObjectArray<std::string*> m_strings;
 
 	btAlignedObjectArray<btCollisionShape*>	m_collisionShapes;
@@ -1692,11 +1691,11 @@ struct PhysicsServerCommandProcessorInternalData
 			m_pluginManager.registerStaticLinkedPlugin("vrSyncPlugin", initPlugin_vrSyncPlugin, exitPlugin_vrSyncPlugin, executePluginCommand_vrSyncPlugin, preTickPluginCallback_vrSyncPlugin, 0, 0);
 #endif //STATIC_LINK_VR_PLUGIN
 
-#ifdef STATIC_PD_CONTROL_PLUGIN
+#ifndef SKIP_STATIC_PD_CONTROL_PLUGIN
 		{
-			m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, postTickPluginCallback_pdControlPlugin, 0);
+			m_pdControlPlugin = m_pluginManager.registerStaticLinkedPlugin("pdControlPlugin", initPlugin_pdControlPlugin, exitPlugin_pdControlPlugin, executePluginCommand_pdControlPlugin, preTickPluginCallback_pdControlPlugin, 0, 0);
 		}
-#endif //STATIC_PD_CONTROL_PLUGIN
+#endif //SKIP_STATIC_PD_CONTROL_PLUGIN
 
 
 #ifndef SKIP_STATIC_TINYRENDERER_PLUGIN
@@ -2482,12 +2481,13 @@ void PhysicsServerCommandProcessor::deleteDynamicsWorld()
 	}
 	m_data->m_worldImporters.clear();
 
+#ifdef ENABLE_LINK_MAPPER
 	for (int i=0;i<m_data->m_urdfLinkNameMapper.size();i++)
 	{
 		delete m_data->m_urdfLinkNameMapper[i];
 	}
 	m_data->m_urdfLinkNameMapper.clear();
-
+#endif //ENABLE_LINK_MAPPER
 
 	for (int i=0;i<m_data->m_strings.size();i++)
 	{
@@ -3010,10 +3010,12 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
 	btMultiBody* mb = bodyHandle? bodyHandle->m_multiBody:0;   
     if (mb)
     {
-        UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
-        m_data->m_urdfLinkNameMapper.push_back(util);
+        UrdfLinkNameMapUtil utilBla;
+		UrdfLinkNameMapUtil* util = &utilBla;
+		btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
+
         util->m_mb = mb;
-        util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
+        util->m_memSerializer = &ser;
 		util->m_memSerializer->startSerialization();
 
         //disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
@@ -3046,9 +3048,11 @@ int PhysicsServerCommandProcessor::createBodyInfoStream(int bodyUniqueId, char*
 		btRigidBody* rb = bodyHandle? bodyHandle->m_rigidBody :0;   
 		if (rb)
 		{
-			UrdfLinkNameMapUtil* util = new UrdfLinkNameMapUtil;
-			m_data->m_urdfLinkNameMapper.push_back(util);
-			util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
+			UrdfLinkNameMapUtil utilBla;
+			UrdfLinkNameMapUtil* util = &utilBla;
+			btDefaultSerializer ser(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
+
+			util->m_memSerializer = &ser;
 			util->m_memSerializer->startSerialization();
 			util->m_memSerializer->registerNameForPointer(bodyHandle->m_rigidBody,bodyHandle->m_bodyName.c_str());
 			//disable serialization of the collision objects (they are too big, and the client likely doesn't need them);
@@ -6049,10 +6053,8 @@ bool PhysicsServerCommandProcessor::processCreateMultiBodyCommand(const struct S
 				serverStatusOut.m_type = CMD_CREATE_MULTI_BODY_COMPLETED;
 
 				int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
-				if (m_data->m_urdfLinkNameMapper.size())
-				{
-					serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
-				}
+				serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
+
 				serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
 				InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 				strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());
@@ -6120,12 +6122,14 @@ bool PhysicsServerCommandProcessor::processLoadURDFCommand(const struct SharedMe
 		serverStatusOut.m_type = CMD_URDF_LOADING_COMPLETED;
 
 		int streamSizeInBytes = createBodyInfoStream(bodyUniqueId, bufferServerToClient, bufferSizeInBytes);
+		serverStatusOut.m_numDataStreamBytes = streamSizeInBytes;
 
-
+#ifdef ENABLE_LINK_MAPPER
 		if (m_data->m_urdfLinkNameMapper.size())
 		{
 			serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
 		}
+#endif
 		serverStatusOut.m_dataStreamArguments.m_bodyUniqueId = bodyUniqueId;
 		InternalBodyData* body = m_data->m_bodyHandles.getHandle(bodyUniqueId);
 		strcpy(serverStatusOut.m_dataStreamArguments.m_bodyName, body->m_bodyName.c_str());

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp

@@ -1,6 +1,5 @@
 #include "b3RobotSimulatorClientAPI_NoDirect.h"
 
-
 #include "PhysicsClientC_API.h"
 #include "b3RobotSimulatorClientAPI_InternalData.h"
 
@@ -35,71 +34,7 @@ b3RobotSimulatorClientAPI_NoDirect::~b3RobotSimulatorClientAPI_NoDirect()
 }
 
 
-bool b3RobotSimulatorClientAPI_NoDirect::connect(int mode, const std::string& hostName, int portOrKey)
-{
-	if (m_data->m_physicsClientHandle)
-	{
-		b3Warning("Already connected, disconnect first.");
-		return false;
-	}
-	b3PhysicsClientHandle sm = 0;
-	int udpPort = 1234;
-	int tcpPort = 6667;
-	int key = SHARED_MEMORY_KEY;
-	bool connected = false;
-
-	switch (mode)
-	{
-
-	case eCONNECT_DIRECT:
-		{
-			break;
-		}
-	case eCONNECT_SHARED_MEMORY:
-		{
-			if (portOrKey >= 0)
-			{
-				key = portOrKey;
-			}
-			sm = b3ConnectSharedMemory(key);
-			break;
-		}
-	case eCONNECT_UDP:
-		{
-			if (portOrKey >= 0)
-			{
-				udpPort = portOrKey;
-			}
-
-			break;
-		}
-	case eCONNECT_TCP:
-		{
-			if (portOrKey >= 0)
-			{
-				tcpPort = portOrKey;
-			}
-			break;
-		}
-
-	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;
-}
 
 bool b3RobotSimulatorClientAPI_NoDirect::isConnected() const
 {

examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h

@@ -419,7 +419,8 @@ class b3RobotSimulatorClientAPI_NoDirect
 	b3RobotSimulatorClientAPI_NoDirect();
 	virtual ~b3RobotSimulatorClientAPI_NoDirect();
 
-	bool connect(int mode, const std::string& hostName = "localhost", int portOrKey = -1);
+	//No 'connect', use setInternalData to bypass the connect method, pass an existing client
+	virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data);
 
 	void disconnect();
 
@@ -580,7 +581,6 @@ class b3RobotSimulatorClientAPI_NoDirect
 
 	bool getVisualShapeData(int bodyUniqueId, b3VisualShapeInformation &visualShapeInfo);
 
-	virtual void setInternalData(struct b3RobotSimulatorClientAPI_InternalData* data);
 
 };
 

examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp

@@ -4,6 +4,7 @@
 #include "../b3PluginContext.h"
 #include <stdio.h>
 
+
 #include "LinearMath/btScalar.h"
 #include "LinearMath/btAlignedObjectArray.h"
 
@@ -52,31 +53,51 @@ B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext*
 {
 	//apply pd control here, apply forces using the PD gains
 	MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
+
 	for (int i = 0; i < obj->m_controllers.size(); i++)
 	{
 		const MyPDControl& pdControl = obj->m_controllers[i];
-		//compute torque
-		//apply torque
+
+		int dof1 = 0;
+		b3JointSensorState actualState;
+		if (obj->m_api.getJointState(pdControl.m_objectUniqueId, pdControl.m_linkIndex,&actualState))
+		{
+			if (pdControl.m_maxForce>0)
+			{
+				//compute torque
+				btScalar qActual = actualState.m_jointPosition;
+				btScalar qdActual = actualState.m_jointVelocity;
+
+				btScalar positionError = (pdControl.m_desiredPosition -qActual);
+				double desiredVelocity = 0;
+				btScalar velocityError = (pdControl.m_desiredVelocity-qdActual);
+
+				btScalar force = pdControl.m_kp * positionError + pdControl.m_kd*velocityError;
+
+				btClamp(force,-pdControl.m_maxForce,pdControl.m_maxForce);
+
+				//apply torque
+				b3RobotSimulatorJointMotorArgs args(CONTROL_MODE_TORQUE);
+				args.m_maxTorqueValue = force;
+				obj->m_api.setJointMotorControl(pdControl.m_objectUniqueId, pdControl.m_linkIndex, args);
+			}
+		}
+
 	}
-	//for each registered pd controller, execute PD control
 
 	return 0;
 }
 
 
-B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context)
-{
-	MyPDControlContainer* obj = (MyPDControlContainer* )context->m_userPointer;
-	obj->m_testData++;
-	return 0;
-}
 
 B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* context, const struct b3PluginArguments* arguments)
 {
 	MyPDControlContainer* obj = (MyPDControlContainer*)context->m_userPointer;
-
+
+	obj->m_api.syncBodies();
+
 	int numObj = obj->m_api.getNumBodies();
-	printf("numObj = %d\n", numObj);
+	//printf("numObj = %d\n", numObj);
 
 	//protocol:
 	//first int is the type of command
@@ -89,20 +110,6 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
 
 	switch (arguments->m_ints[0])
 	{
-	case eAddPDControl:
-	{
-		if (arguments->m_numFloats < 5)
-			return -1;
-		MyPDControl controller;
-		controller.m_desiredPosition = arguments->m_floats[0];
-		controller.m_desiredVelocity = arguments->m_floats[1];
-		controller.m_kd = arguments->m_floats[2];
-		controller.m_kp = arguments->m_floats[3];
-		controller.m_maxForce = arguments->m_floats[4];
-		controller.m_objectUniqueId = arguments->m_ints[1];
-		controller.m_linkIndex = arguments->m_ints[2];
-		obj->m_controllers.push_back(controller);
-	}
 	case	eSetPDControl:
 	{
 		if (arguments->m_numFloats < 5)
@@ -116,14 +123,19 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
 		controller.m_objectUniqueId = arguments->m_ints[1];
 		controller.m_linkIndex = arguments->m_ints[2];
 
+		int foundIndex = -1;
 		for (int i = 0; i < obj->m_controllers.size(); i++)
 		{
 			if (obj->m_controllers[i].m_objectUniqueId == controller.m_objectUniqueId && obj->m_controllers[i].m_linkIndex == controller.m_linkIndex)
 			{
 				obj->m_controllers[i] = controller;
-				break;
+				foundIndex=i;
 			}
 		}
+		if (foundIndex<0)
+		{
+			obj->m_controllers.push_back(controller);
+		}
 		break;
 	}
 	case	eRemovePDControl: