nearly working

examples/SharedMemory/plugins/eglPlugin/bullet.py

@@ -5,38 +5,81 @@ import subprocess
 import pybullet as p
 from pdb import set_trace
 import matplotlib.pyplot as plt
+import numpy as np
 #subprocess.call(["hardening-check", p.__file__])
 
+
+
 p.connect(p.DIRECT)
 logId = p.startStateLogging(p.STATE_LOGGING_PROFILE_TIMINGS, "debugTimings")
 
-plugin_fn = '/home/argusm/lang/bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so'
-plugin = p.loadPlugin(plugin_fn,"_tinyRendererPlugin")
-if plugin < 0:
-    print("\nPlugin Failed to load!\n")
-    sys.exit()
+plugin = True
+if plugin:
+    plugin_fn = '/home/argusm/lang/bullet3/build/lib.linux-x86_64-3.5/eglRenderer.cpython-35m-x86_64-linux-gnu.so'
+    plugin = p.loadPlugin(plugin_fn,"_tinyRendererPlugin")
+    if plugin < 0:
+        print("\nPlugin Failed to load!\n")
+        sys.exit()
+    print("plugin =",plugin)
 
-print("plugin =",plugin)
-
-path = '/home/argusm/lang/bullet3/examples/pybullet/gym/pybullet_data/r2d2.urdf'
-path = '/home/argusm/lang/gym-grasping/gym_grasping/robots/models/kuka_iiwa/kuka_weiss_bolt.sdf'
-#p.loadURDF(path)
-p.loadSDF(path)
+path = '/home/argusm/lang/bullet3/examples/pybullet/gym/pybullet_data/duck_vhacd.urdf'
+p.loadURDF(path,globalScaling=12)
+#path = '/home/argusm/lang/gym-grasping/gym_grasping/robots/models/kuka_iiwa/kuka_weiss_bolt.sdf'
+#p.loadSDF(path)
 
 start = time.time()
 
-plot = True
+camTargetPos = [0,0,0]
+upAxisIndex = 2
+nearPlane = 0.01
+farPlane = 100
+camDistance = 2
+pixelWidth = 128
+pixelHeight = 128
+fov = 60
+
+plot = False
+anim = True
+if plot:
+    plt.ion()
+if anim:
+    import matplotlib.animation as manimation
+    FFMpegWriter = manimation.writers['ffmpeg']
+    metadata = dict(title='Movie Test', artist='Matplotlib',
+                    comment='Movie support!')
+    writer = FFMpegWriter(fps=15, metadata=metadata)
+if plot or anim:
+    fig = plt.figure()
+    img = np.random.rand(pixelWidth,pixelHeight)
+    image = plt.imshow(img,interpolation='none',animated=True,label="blah")
+    ax = plt.gca()
+    ax.set_axis_off()
+    ax.set_aspect('equal')
+    plt.subplots_adjust(wspace=0, hspace=0, left=0, bottom=0, right=1, top=1)
+
+
 try:
-    for i in range(10):
-        hight, width, img_arr, deept_arr, obj_arr = p.getCameraImage(128,128)
-        if plot:
-            plt.imshow(img_arr[:,:,:3])
-            plt.show()
-        if i % 100 == 0 and i > 0:
-            print("FPS",100/(time.time()-start))
-        start = time.time()
+    iter = range(0,360,10)
+    with writer.saving(fig, "debug.mp4", len(iter)):
+        for i,yaw in enumerate(iter):
+            viewMatrix = p.computeViewMatrixFromYawPitchRoll(camTargetPos, camDistance, 0, yaw-90, 0, upAxisIndex)
+            aspect = pixelWidth / pixelHeight;
+            projectionMatrix = p.computeProjectionMatrixFOV(fov, aspect, nearPlane, farPlane);
+
+            hight, width, img_arr, deept_arr, obj_arr = p.getCameraImage(pixelWidth,pixelHeight,viewMatrix,projectionMatrix)
+            if plot:
+                image.set_data(img_arr)#np_img_arr)
+                ax.plot([0])
+                #plt.draw()
+                #plt.show()
+                plt.pause(0.01)
+            if anim:
+                image.set_data(img_arr)#np_img_arr)
+                ax.plot([0])
+                writer.grab_frame()
+
+            if i % 100 == 0 and i > 0:
+                print("FPS",100/(time.time()-start))
+            start = time.time()
 finally:
     p.stopStateLogging(logId)
-
-
-