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

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);