enable pdControlPlugin by default (requires pdControlPlugin.cpp and b3RobotSimulatorClientAPI_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 #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;
+        UrdfLinkNameMapUtil utilBla;
-        m_data->m_urdfLinkNameMapper.push_back(util);
+		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;
+			UrdfLinkNameMapUtil utilBla;
-			m_data->m_urdfLinkNameMapper.push_back(util);
+			UrdfLinkNameMapUtil* util = &utilBla;
-			util->m_memSerializer = new btDefaultSerializer(bufferSizeInBytes ,(unsigned char*)bufferServerToClient);
+			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 = streamSizeInBytes;
-				{
+				
-					serverStatusOut.m_numDataStreamBytes = m_data->m_urdfLinkNameMapper.at(m_data->m_urdfLinkNameMapper.size()-1)->m_memSerializer->getCurrentBufferSize();
-				}
 				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 @@ public:
 	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 @@ public:
 
 	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:

examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h

@@ -16,14 +16,12 @@ B3_SHARED_API int executePluginCommand_pdControlPlugin(struct b3PluginContext* c
 ///
 enum PDControlCommandEnum
 {
-	eAddPDControl = 1,
+	eSetPDControl = 1,
-	eSetPDControl = 2,
+	eRemovePDControl = 2,
-	eRemovePDControl = 4,
 };
 
 //all the APIs below are optional
 B3_SHARED_API int preTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
-B3_SHARED_API int postTickPluginCallback_pdControlPlugin(struct b3PluginContext* context);
 
 
 #ifdef __cplusplus

examples/SharedMemory/premake4.lua

@@ -20,6 +20,8 @@ language "C++"
 
 myfiles = 
 {
+	"b3RobotSimulatorClientAPI_NoDirect.cpp",
+	"b3RobotSimulatorClientAPI_NoDirect.h",
 	"IKTrajectoryHelper.cpp",
 	"IKTrajectoryHelper.h",
 	"PhysicsClient.cpp",
@@ -58,6 +60,8 @@ myfiles =
 	"PhysicsServerCommandProcessor.h",
 	"b3PluginManager.cpp",
 	"b3PluginManager.h",
+	"plugins/pdControlPlugin/pdControlPlugin.cpp",
+	"plugins/pdControlPlugin/pdControlPlugin.h",
 	"../OpenGLWindow/SimpleCamera.cpp",
 	"../OpenGLWindow/SimpleCamera.h",
 	"../Importers/ImportURDFDemo/ConvertRigidBodies2MultiBody.h",

examples/SharedMemory/tcp/premake4.lua

@@ -91,7 +91,12 @@ myfiles =
 	"../PhysicsServerCommandProcessor.h",
 	"../b3PluginManager.cpp",
 	"../PhysicsDirect.cpp",
+	"../PhysicsClientC_API.cpp",
 	"../PhysicsClient.cpp",
+	"../plugins/pdControlPlugin/pdControlPlugin.cpp",
+	"../plugins/pdControlPlugin/pdControlPlugin.h",
+	"../b3RobotSimulatorClientAPI_NoDirect.cpp",
+	"../b3RobotSimulatorClientAPI_NoDirect.h",
   "../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
 	"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",
 	"../../TinyRenderer/geometry.cpp",

examples/SharedMemory/udp/premake4.lua

@@ -77,6 +77,10 @@ language "C++"
 
 myfiles = 
 {
+	"../plugins/pdControlPlugin/pdControlPlugin.cpp",
+	"../plugins/pdControlPlugin/pdControlPlugin.h",
+	"../b3RobotSimulatorClientAPI_NoDirect.cpp",
+	"../b3RobotSimulatorClientAPI_NoDirect.h",
 	"../IKTrajectoryHelper.cpp",
 	"../IKTrajectoryHelper.h",
 	"../SharedMemoryCommands.h",
@@ -85,6 +89,7 @@ myfiles =
 	"../PhysicsServerCommandProcessor.h",
 	"../b3PluginManager.cpp",
 	"../PhysicsDirect.cpp",
+	"../PhysicsClientC_API.cpp",
 	"../PhysicsClient.cpp",
 	"../plugins/tinyRendererPlugin/tinyRendererPlugin.cpp",
 	"../plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp",

examples/TwoJoint/CMakeLists.txt

@@ -10,6 +10,10 @@ INCLUDE_DIRECTORIES(
 
 SET(RobotSimulator_SRCS
 		TwoJointMain.cpp
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
+		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
+		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
 		../../examples/SharedMemory/IKTrajectoryHelper.cpp
 		../../examples/SharedMemory/IKTrajectoryHelper.h
 		../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

examples/pybullet/CMakeLists.txt

@@ -15,6 +15,10 @@ ENDIF()
 
 SET(pybullet_SRCS
 	pybullet.c
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
+		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
+		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
 		../../examples/SharedMemory/IKTrajectoryHelper.cpp
 		../../examples/SharedMemory/IKTrajectoryHelper.h
 		../../examples/ExampleBrowser/InProcessExampleBrowser.cpp

examples/pybullet/examples/pdControl.py

@@ -0,0 +1,62 @@
+
+import pybullet as p
+import time
+
+useMaximalCoordinates=False
+p.connect(p.GUI)
+pole = p.loadURDF("cartpole.urdf", useMaximalCoordinates=useMaximalCoordinates)
+for i in range (p.getNumJoints(pole)):
+	#disable default constraint-based motors
+	p.setJointMotorControl2(pole,i,p.POSITION_CONTROL,targetPosition=0,force=0)
+	print("joint",i,"=",p.getJointInfo(pole,i))
+
+
+desiredPosCartId = p.addUserDebugParameter("desiredPosCart",-10,10,2)
+desiredVelCartId = p.addUserDebugParameter("desiredVelCart",-10,10,0)
+kpCartId = p.addUserDebugParameter("kpCart",0,500,300)
+kdCartId = p.addUserDebugParameter("kpCart",0,300,150)
+maxForceCartId = p.addUserDebugParameter("maxForceCart",0,5000,1000)
+
+
+desiredPosPoleId = p.addUserDebugParameter("desiredPosPole",-10,10,0)
+desiredVelPoleId = p.addUserDebugParameter("desiredVelPole",-10,10,0)
+kpPoleId = p.addUserDebugParameter("kpPole",0,500,200)
+kdPoleId = p.addUserDebugParameter("kpPole",0,300,100)
+maxForcePoleId = p.addUserDebugParameter("maxForcePole",0,5000,1000)
+
+pd = p.loadPlugin("pdControlPlugin")
+
+	
+p.setGravity(0,0,0)
+
+useRealTimeSim = True
+
+p.setRealTimeSimulation(useRealTimeSim)
+	
+p.setTimeStep(0.001)
+	
+	
+while p.isConnected():
+	if (pd>=0):
+		desiredPosCart = p.readUserDebugParameter(desiredPosCartId)
+		desiredVelCart = p.readUserDebugParameter(desiredVelCartId)
+		kpCart = p.readUserDebugParameter(kpCartId)
+		kdCart = p.readUserDebugParameter(kdCartId)
+		maxForceCart = p.readUserDebugParameter(maxForceCartId)
+		link = 0
+		p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosCart, desiredVelCart, kdCart, kpCart, maxForceCart])
+		
+		desiredPosPole = p.readUserDebugParameter(desiredPosPoleId)
+		desiredVelPole = p.readUserDebugParameter(desiredVelPoleId)
+		kpPole = p.readUserDebugParameter(kpPoleId)
+		kdPole = p.readUserDebugParameter(kdPoleId)
+		maxForcePole = p.readUserDebugParameter(maxForcePoleId)
+		link = 1
+		p.executePluginCommand(pd,"test",[1, pole, link], [desiredPosPole, desiredVelPole, kdPole, kpPole, maxForcePole])
+		
+
+	if (not useRealTimeSim):
+		p.stepSimulation()
+		time.sleep(1./240.)
+	
+	

examples/pybullet/gym/pybullet_data/random_urdfs/948/948.urdf

@@ -0,0 +1,31 @@
+<robot name="blob948">
+  <link name="random_obj_948">
+    <contact>
+      <lateral_friction value="1.0"/>
+      <rolling_friction value="0.0"/>
+      <inertia_scaling value="3.0"/>
+      <contact_cfm value="0.0"/>
+      <contact_erp value="1.0"/>
+    </contact>
+    <inertial>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <mass value="0.1"/>
+      <inertia ixx="1" ixy="0" ixz="0" iyy="1" iyz="0" izz="0"/>
+    </inertial>
+    <visual>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="948.obj" scale="0.015 0.015 0.015"/>
+      </geometry>
+      <material name="blockmat">
+        <color rgba="0.26 0.59 0.61 1"/>
+      </material>
+    </visual>
+    <collision>
+      <origin rpy="0 0 0" xyz="0 0 0"/>
+      <geometry>
+        <mesh filename="948.obj" scale="0.015 0.015 0.015"/>
+      </geometry>
+    </collision>
+  </link>
+</robot>

examples/pybullet/premake4.lua

@@ -103,6 +103,8 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/TinyRenderer/our_gl.cpp",
 			"../../examples/TinyRenderer/TinyRenderer.cpp",
 			"../../examples/SharedMemory/InProcessMemory.cpp",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
 			"../../examples/SharedMemory/PhysicsClient.cpp",
 			"../../examples/SharedMemory/PhysicsClient.h",
 			"../../examples/SharedMemory/PhysicsServer.cpp",
@@ -153,6 +155,8 @@ if not _OPTIONS["no-enet"] then
 			"../../examples/MultiThreading/b3PosixThreadSupport.cpp",
 			"../../examples/MultiThreading/b3Win32ThreadSupport.cpp",
 			"../../examples/MultiThreading/b3ThreadSupportInterface.cpp",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			}
 			
 if (_OPTIONS["enable_static_vr_plugin"]) then

examples/pybullet/pybullet.c

@@ -8316,7 +8316,14 @@ static PyObject* pybullet_executePluginCommand(PyObject* self,
 			float val = pybullet_internalGetFloatFromSequence(seqFloatArgs,i);
 			b3CustomCommandExecuteAddFloatArgument(command, val);
 		}
-		
+		if (seqFloatArgs)
+		{
+			Py_DECREF(seqFloatArgs);
+		}
+		if (seqIntArgs)
+		{
+			Py_DECREF(seqIntArgs);
+		}
 	}
 	
 	statusHandle = b3SubmitClientCommandAndWaitStatus(sm, command);

setup.py

@@ -49,6 +49,8 @@ sources = ["examples/pybullet/pybullet.c"]\
 +["examples/TinyRenderer/tgaimage.cpp"]\
 +["examples/TinyRenderer/our_gl.cpp"]\
 +["examples/TinyRenderer/TinyRenderer.cpp"]\
++["examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp"]\
++["examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp"]\
 +["examples/SharedMemory/IKTrajectoryHelper.cpp"]\
 +["examples/SharedMemory/InProcessMemory.cpp"]\
 +["examples/SharedMemory/PhysicsClient.cpp"]\

test/SharedMemory/CMakeLists.txt

@@ -29,66 +29,69 @@ ENDIF()
 	ADD_EXECUTABLE(Test_PhysicsClientServer
 		gtestwrap.cpp
 		../../examples/SharedMemory/PhysicsClient.cpp
-												../../examples/SharedMemory/IKTrajectoryHelper.cpp
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp
-												../../examples/SharedMemory/IKTrajectoryHelper.h
+		../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h
-                        ../../examples/SharedMemory/PhysicsClient.h
+		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp
-                        ../../examples/SharedMemory/PhysicsServer.cpp
+		../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h
-                        ../../examples/SharedMemory/PhysicsServer.h
+		../../examples/SharedMemory/IKTrajectoryHelper.cpp
-                        ../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
+		../../examples/SharedMemory/IKTrajectoryHelper.h
-                        ../../examples/SharedMemory/PhysicsServerSharedMemory.h
+		../../examples/SharedMemory/PhysicsClient.h
-                        ../../examples/SharedMemory/PhysicsDirect.cpp
+		../../examples/SharedMemory/PhysicsServer.cpp
-                        ../../examples/SharedMemory/PhysicsDirect.h
+		../../examples/SharedMemory/PhysicsServer.h
-                        ../../examples/SharedMemory/PhysicsDirectC_API.cpp
+		../../examples/SharedMemory/PhysicsServerSharedMemory.cpp
-                        ../../examples/SharedMemory/PhysicsDirectC_API.h
+		../../examples/SharedMemory/PhysicsServerSharedMemory.h
-                        ../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
+		../../examples/SharedMemory/PhysicsDirect.cpp
-                        ../../examples/SharedMemory/PhysicsServerCommandProcessor.h
+		../../examples/SharedMemory/PhysicsDirect.h
-                        ../../examples/SharedMemory/b3PluginManager.cpp
+		../../examples/SharedMemory/PhysicsDirectC_API.cpp
-                        ../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
+		../../examples/SharedMemory/PhysicsDirectC_API.h
-                        ../../examples/SharedMemory/PhysicsClientSharedMemory.h
+		../../examples/SharedMemory/PhysicsServerCommandProcessor.cpp
-						../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
+		../../examples/SharedMemory/PhysicsServerCommandProcessor.h
-                        ../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
+		../../examples/SharedMemory/b3PluginManager.cpp
-                        
+		../../examples/SharedMemory/PhysicsClientSharedMemory.cpp
-						../../examples/SharedMemory/PhysicsClientC_API.cpp
+		../../examples/SharedMemory/PhysicsClientSharedMemory.h
-                        ../../examples/SharedMemory/PhysicsClientC_API.h
+		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp
-			../../examples/SharedMemory/PhysicsLoopBack.cpp
+		../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h
-                        ../../examples/SharedMemory/PhysicsLoopBack.h
+		../../examples/SharedMemory/PhysicsClientC_API.cpp
-                        ../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
+		../../examples/SharedMemory/PhysicsClientC_API.h
-                        ../../examples/SharedMemory/PhysicsLoopBackC_API.h
+		../../examples/SharedMemory/PhysicsLoopBack.cpp
-                        ../../examples/SharedMemory/Win32SharedMemory.cpp
+		../../examples/SharedMemory/PhysicsLoopBack.h
-                        ../../examples/SharedMemory/Win32SharedMemory.h
+		../../examples/SharedMemory/PhysicsLoopBackC_API.cpp
-                        ../../examples/SharedMemory/PosixSharedMemory.cpp
+		../../examples/SharedMemory/PhysicsLoopBackC_API.h
-                        ../../examples/SharedMemory/PosixSharedMemory.h
+		../../examples/SharedMemory/Win32SharedMemory.cpp
-                        ../../examples/Utils/b3ResourcePath.cpp
+		../../examples/SharedMemory/Win32SharedMemory.h
-                        ../../examples/Utils/b3ResourcePath.h
+		../../examples/SharedMemory/PosixSharedMemory.cpp
-                        ../../examples/Utils/b3Clock.cpp
+		../../examples/SharedMemory/PosixSharedMemory.h
-                        ../../examples/Utils/b3Clock.h
+		../../examples/Utils/b3ResourcePath.cpp
-												../../examples/Utils/ChromeTraceUtil.cpp
+		../../examples/Utils/b3ResourcePath.h
-												../../examples/Utils/ChromeTraceUtil.h
+		../../examples/Utils/b3Clock.cpp
-						../../examples/Utils/RobotLoggingUtil.cpp
+		../../examples/Utils/b3Clock.h
-                        ../../examples/Utils/RobotLoggingUtil.h
+		../../examples/Utils/ChromeTraceUtil.cpp
-					    ../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
+		../../examples/Utils/ChromeTraceUtil.h
-							../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
+		../../examples/Utils/RobotLoggingUtil.cpp
-						../../examples/OpenGLWindow/SimpleCamera.cpp
+		../../examples/Utils/RobotLoggingUtil.h
-						../../examples/OpenGLWindow/SimpleCamera.h
+		../../examples/SharedMemory/plugins/tinyRendererPlugin/tinyRendererPlugin.cpp
-						../../examples/TinyRenderer/geometry.cpp
+		../../examples/SharedMemory/plugins/tinyRendererPlugin/TinyRendererVisualShapeConverter.cpp
-						../../examples/TinyRenderer/model.cpp
+		../../examples/OpenGLWindow/SimpleCamera.cpp
-						../../examples/TinyRenderer/tgaimage.cpp
+		../../examples/OpenGLWindow/SimpleCamera.h
-						../../examples/TinyRenderer/our_gl.cpp
+		../../examples/TinyRenderer/geometry.cpp
-						../../examples/TinyRenderer/TinyRenderer.cpp
+		../../examples/TinyRenderer/model.cpp
-                        ../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
+		../../examples/TinyRenderer/tgaimage.cpp
-                        ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
+		../../examples/TinyRenderer/our_gl.cpp
-                        ../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
+		../../examples/TinyRenderer/TinyRenderer.cpp
-                        ../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
+		../../examples/ThirdPartyLibs/tinyxml2/tinyxml2.cpp
-                        ../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
+		../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.cpp
-                        ../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
+		../../examples/ThirdPartyLibs/Wavefront/tiny_obj_loader.h
-                        ../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
+		../../examples/Importers/ImportColladaDemo/LoadMeshFromCollada.cpp
-                        ../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
+		../../examples/Importers/ImportObjDemo/LoadMeshFromObj.cpp
-                        ../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
+		../../examples/Importers/ImportObjDemo/Wavefront2GLInstanceGraphicsShape.cpp
-                        ../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
+		../../examples/Importers/ImportMJCFDemo/BulletMJCFImporter.cpp
-                        ../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+		../../examples/Importers/ImportURDFDemo/BulletUrdfImporter.cpp
-                        ../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+		../../examples/Importers/ImportURDFDemo/MyMultiBodyCreator.cpp
-			../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+		../../examples/Importers/ImportURDFDemo/URDF2Bullet.cpp
-			../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
+		../../examples/Importers/ImportURDFDemo/UrdfParser.cpp
+		../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp
+		../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp
+		../../examples/ThirdPartyLibs/stb_image/stb_image.cpp
                         
 	)
 

test/SharedMemory/premake4.lua

@@ -170,6 +170,10 @@ project ("Test_PhysicsServerLoopBack")
 			
 		files {
 			"test.c",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
 			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/SharedMemory/PhysicsClient.cpp",
@@ -186,6 +190,8 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/SharedMemory/PhysicsLoopBack.h",
 			"../../examples/SharedMemory/PhysicsLoopBackC_API.cpp",
 			"../../examples/SharedMemory/PhysicsLoopBackC_API.h",
+			"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.cpp",
+			"../../examples/SharedMemory/PhysicsClientSharedMemory_C_API.h",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.cpp",
 			"../../examples/SharedMemory/PhysicsClientSharedMemory.h",
 			"../../examples/SharedMemory/PhysicsClientC_API.cpp",
@@ -224,7 +230,7 @@ project ("Test_PhysicsServerLoopBack")
 			"../../examples/Importers/ImportURDFDemo/UrdfParser.cpp",
 			"../../examples/Importers/ImportURDFDemo/urdfStringSplit.cpp",
 			"../../examples/Importers/ImportMeshUtility/b3ImportMeshUtility.cpp",
-                        "../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
+			"../../examples/ThirdPartyLibs/stb_image/stb_image.cpp",
 	}
 	
 if (_OPTIONS["enable_static_plugins"]) then
@@ -256,6 +262,10 @@ end
 			
 		files {
 			"test.c",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
 			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/SharedMemory/PhysicsClient.cpp",
@@ -360,6 +370,10 @@ project ("Test_PhysicsServerInProcessExampleBrowser")
 
 		files {
 			"test.c",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.cpp",
+			"../../examples/SharedMemory/b3RobotSimulatorClientAPI_NoDirect.h",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.cpp",
+			"../../examples/SharedMemory/plugins/pdControlPlugin/pdControlPlugin.h",
 			"../../examples/SharedMemory/IKTrajectoryHelper.cpp",
 			"../../examples/SharedMemory/IKTrajectoryHelper.h",
 			"../../examples/ExampleBrowser/InProcessExampleBrowser.cpp",