86 lines
2.6 KiB
Python
86 lines
2.6 KiB
Python
import os
|
|
import sys
|
|
import time
|
|
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 = 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)
|
|
|
|
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()
|
|
|
|
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:
|
|
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)
|