abstract phys renderer
This commit is contained in:
hzyjerry
parent
cd9dbe8e7e
commit
bfa0b87516
|
|
@ -16,179 +16,167 @@ from numpy import sin, cos
|
|||
|
||||
PHYSICS_FIRST = True
|
||||
|
||||
def camera_init_orientation(quat):
|
||||
to_z_facing = euler.euler2quat(np.pi/2, np.pi, 0)
|
||||
return quaternions.qmult(to_x_facing, quat_wxyz)
|
||||
class PhysRenderer(object):
|
||||
|
||||
def setPosViewOrientation(objectUid, pos, rot):
|
||||
return
|
||||
def __init__(self, datapath, model_id, framePerSec):
|
||||
context = zmq.Context()
|
||||
self.visn_socket = context.socket(zmq.REQ)
|
||||
self.visn_socket.bind("tcp://*:5556")
|
||||
self.debug_mode = True
|
||||
self.debug_sliders = {}
|
||||
|
||||
## Turn on p.GUI for visualization
|
||||
## Turn on p.GUI for headless mode
|
||||
if self.debug_mode:
|
||||
p.connect(p.GUI)
|
||||
self._startDebugRoomMap()
|
||||
else:
|
||||
p.connect(p.DIRECT)
|
||||
|
||||
def getCollisionFromUpdate():
|
||||
message = socket.recv().decode("utf-8")
|
||||
obj_path = os.path.join(datapath, model_id, "modeldata", 'out_z_up.obj')
|
||||
|
||||
x, y, z, r_w, r_x, r_y, r_z = map(float, message.split())
|
||||
p.setRealTimeSimulation(0)
|
||||
boundaryUid = p.createCollisionShape(p.GEOM_MESH, fileName=obj_path, meshScale=[1, 1, 1], flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
|
||||
print("Exterior boundary", boundaryUid)
|
||||
p.changeVisualShape(boundaryUid, -1, rgbaColor=[1, 1, 1, 0.5])
|
||||
p.createMultiBody(0,0)
|
||||
|
||||
p.resetBasePositionAndOrientation(objectUid, [x, y, z], [r_w, r_x, r_y, r_z])
|
||||
p.stepSimulation()
|
||||
print("step simulation done")
|
||||
collisions = p.getContactPoints(boundaryUid, objectUid)
|
||||
if len(collisions) == 0:
|
||||
print("No collisions")
|
||||
else:
|
||||
print("Collisions!")
|
||||
print("collision length", len(collisions))
|
||||
socket.send_string(str(len(collisions)))
|
||||
return
|
||||
p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(0)
|
||||
self.framePerSec = framePerSec
|
||||
|
||||
def sendPoseToViewPort(pose):
|
||||
socket.send_string(json.dumps(pose))
|
||||
socket.recv()
|
||||
file_dir = os.path.dirname(__file__)
|
||||
#objectUid = p.loadURDF("models/quadrotor.urdf", globalScaling = 0.8)
|
||||
self.objectUid = p.loadURDF(os.path.join(file_dir, "models/husky.urdf"), globalScaling = 0.8)
|
||||
#p.changeVisualShape(objectUid, -1, rgbaColor=[1, 1, 1, 0.5])
|
||||
|
||||
def getInitialPositionOrientation():
|
||||
print("waiting to receive initial")
|
||||
socket.send_string("Initial")
|
||||
pos, quat = json.loads(socket.recv().decode("utf-8"))
|
||||
print("received initial", pos, quat)
|
||||
return pos, quat
|
||||
pos, quat_xyzw = self._getInitialPositionOrientation()
|
||||
|
||||
def synchronizeWithViewPort():
|
||||
#step
|
||||
view_pose = json.loads(socket.recv().decode("utf-8"))
|
||||
changed = view_pose['changed']
|
||||
## Always send pose from last frame
|
||||
pos, rot = p.getBasePositionAndOrientation(objectUid)
|
||||
v_t = 1 # 1m/s max speed
|
||||
v_r = np.pi/5 # 36 degrees/s
|
||||
self.cart = PhysicsObject(self.objectUid, p, pos, quat_xyzw, v_t, v_r, framePerSec)
|
||||
|
||||
print("receiving changed ?", changed)
|
||||
print("receiving from view", view_pose['pos'])
|
||||
print("original view posit", pos)
|
||||
print("receiving from view", view_pose['quat'])
|
||||
print("original view posit", rot)
|
||||
if changed:
|
||||
## Apply the changes
|
||||
new_pos = view_pose['pos']
|
||||
new_quat = view_pose['quat']
|
||||
#new_quat = [0, 0, 0, 1]
|
||||
p.resetBasePositionAndOrientation(objectUid, new_pos, new_quat)
|
||||
p.stepSimulation()
|
||||
pos, rot = p.getBasePositionAndOrientation(objectUid)
|
||||
print("after applying pose", pos)
|
||||
print("")
|
||||
#print(changed, pos, rot)
|
||||
socket.send_string(json.dumps([pos, rot]))
|
||||
print("Generated cart", self.objectUid)
|
||||
#p.setTimeStep(1.0/framePerSec)
|
||||
p.setTimeStep(1.0/settings.STEPS_PER_SEC)
|
||||
|
||||
def stepNsteps(N, object):
|
||||
for _ in range(N):
|
||||
def _camera_init_orientation(self, quat):
|
||||
to_z_facing = euler.euler2quat(np.pi/2, np.pi, 0)
|
||||
return quaternions.qmult(to_z_facing, quat_wxyz)
|
||||
|
||||
def _setPosViewOrientation(objectUid, pos, rot):
|
||||
return
|
||||
|
||||
def _sendPoseToViewPort(self, pose):
|
||||
self.visn_socket.send_string(json.dumps(pose))
|
||||
self.visn_socket.recv()
|
||||
|
||||
def _getInitialPositionOrientation(self):
|
||||
print("waiting to receive initial")
|
||||
self.visn_socket.send_string("Initial")
|
||||
pos, quat = json.loads(self.visn_socket.recv().decode("utf-8"))
|
||||
print("received initial", pos, quat)
|
||||
return pos, quat
|
||||
|
||||
def _stepNsteps(self, N, pObject):
|
||||
for _ in range(N):
|
||||
p.stepSimulation()
|
||||
pObject.parseActionAndUpdate()
|
||||
|
||||
def _startDebugRoomMap(self):
|
||||
cameraDistSlider = p.addUserDebugParameter("Distance",0,10,7)
|
||||
cameraYawSlider = p.addUserDebugParameter("Camera Yaw",-180,180,0)
|
||||
cameraPitchSlider = p.addUserDebugParameter("Camera Pitch",-90,90,0)
|
||||
self.debug_sliders = {
|
||||
'dist' :cameraDistSlider,
|
||||
'yaw' : cameraYawSlider,
|
||||
'pitch': cameraPitchSlider
|
||||
}
|
||||
|
||||
def renderToScreen(self):
|
||||
startttime = time.time()
|
||||
lasttime = time.time()
|
||||
while (1):
|
||||
## Execute one frame
|
||||
self.cart.getUpdateFromKeyboard()
|
||||
self._sendPoseToViewPort(self.cart.getViewPosAndOrientation())
|
||||
|
||||
simutime = time.time()
|
||||
print('time step', 1.0/settings.STEPS_PER_SEC, 'stepping', settings.STEPS_PER_SEC/framePerSec)
|
||||
self._stepNsteps(int(settings.STEPS_PER_SEC/framePerSec), self.cart)
|
||||
#for _ in range(int(settings.STEPS_PER_SEC/framePerSec)):
|
||||
# p.stepSimulation()
|
||||
|
||||
print("passed time", time.time() - lasttime, "simulation time", time.time() - simutime)
|
||||
lasttime = time.time()
|
||||
print("last time", lasttime, "start time", startttime)
|
||||
if lasttime - startttime > 5:
|
||||
startttime = lasttime
|
||||
self.cart.getUpdateFromKeyboard(restart=True)
|
||||
self._stepNsteps(int(settings.STEPS_PER_SEC/framePerSec), self.cart)
|
||||
if self.debug_mode:
|
||||
cameraDist = p.readUserDebugParameter(self.debug_sliders['dist'])
|
||||
cameraYaw = p.readUserDebugParameter(self.debug_sliders['yaw'])
|
||||
cameraPitch = p.readUserDebugParameter(self.debug_sliders['pitch'])
|
||||
viewMatrix = p.computeViewMatrixFromYawPitchRoll([0, 0, 0], 8, 0, 0, 0, 2)
|
||||
projMatrix = p.computeProjectionMatrix(-0.1, 0.1, -0.1, 0.1, 0.1, 128)
|
||||
p.getCameraImage(256, 256, viewMatrix = viewMatrix, projectionMatrix = projMatrix)
|
||||
p.resetDebugVisualizerCamera(cameraDist, cameraYaw, cameraPitch, [0, 0, 0])
|
||||
time.sleep(0.01)
|
||||
|
||||
## DEPRECATED
|
||||
def _getCollisionFromUpdate(self):
|
||||
message = self.visn_socket.recv().decode("utf-8")
|
||||
|
||||
x, y, z, r_w, r_x, r_y, r_z = map(float, message.split())
|
||||
|
||||
p.resetBasePositionAndOrientation(self.objectUid, [x, y, z], [r_w, r_x, r_y, r_z])
|
||||
p.stepSimulation()
|
||||
object.parseActionAndUpdate()
|
||||
print("step simulation done")
|
||||
collisions = p.getContactPoints(boundaryUid, self.objectUid)
|
||||
if len(collisions) == 0:
|
||||
print("No collisions")
|
||||
else:
|
||||
print("Collisions!")
|
||||
print("collision length", len(collisions))
|
||||
self.visn_socket.send_string(str(len(collisions)))
|
||||
return
|
||||
|
||||
## DEPRECATED
|
||||
def _synchronizeWithViewPort(self):
|
||||
#step
|
||||
view_pose = json.loads(self.visn_socket.recv().decode("utf-8"))
|
||||
changed = view_pose['changed']
|
||||
## Always send pose from last frame
|
||||
pos, rot = p.getBasePositionAndOrientation(self.objectUid)
|
||||
|
||||
print("receiving changed ?", changed)
|
||||
print("receiving from view", view_pose['pos'])
|
||||
print("original view posit", pos)
|
||||
print("receiving from view", view_pose['quat'])
|
||||
print("original view posit", rot)
|
||||
if changed:
|
||||
## Apply the changes
|
||||
new_pos = view_pose['pos']
|
||||
new_quat = view_pose['quat']
|
||||
#new_quat = [0, 0, 0, 1]
|
||||
p.resetBasePositionAndOrientation(self.objectUid, new_pos, new_quat)
|
||||
p.stepSimulation()
|
||||
pos, rot = p.getBasePositionAndOrientation(self.objectUid)
|
||||
print("after applying pose", pos)
|
||||
print("")
|
||||
#print(changed, pos, rot)
|
||||
self.visn_socket.send_string(json.dumps([pos, rot]))
|
||||
|
||||
if __name__ == '__main__':
|
||||
parser = argparse.ArgumentParser()
|
||||
parser.add_argument('--datapath' , required = True, help='dataset path')
|
||||
parser.add_argument('--model' , type = str, default = '', help='path of model')
|
||||
|
||||
opt = parser.parse_args()
|
||||
|
||||
|
||||
context = zmq.Context()
|
||||
socket = context.socket(zmq.REQ)
|
||||
socket.bind("tcp://*:5556")
|
||||
|
||||
## Turn on p.GUI for visualization
|
||||
## Turn on p.GUI for headless mode
|
||||
#p.connect(p.GUI)
|
||||
p.connect(p.DIRECT)
|
||||
|
||||
|
||||
obj_path = os.path.join(opt.datapath, opt.model, "modeldata", 'out_z_up.obj')
|
||||
|
||||
p.setRealTimeSimulation(0)
|
||||
boundaryUid = p.createCollisionShape(p.GEOM_MESH, fileName=obj_path, meshScale=[1, 1, 1], flags=p.GEOM_FORCE_CONCAVE_TRIMESH)
|
||||
print("Exterior boundary", boundaryUid)
|
||||
p.changeVisualShape(boundaryUid, -1, rgbaColor=[1, 1, 1, 0.5])
|
||||
|
||||
p.createMultiBody(0,0)
|
||||
|
||||
sphereRadius = 0.05
|
||||
colSphereId = p.createCollisionShape(p.GEOM_SPHERE,radius=sphereRadius)
|
||||
colBoxId = p.createCollisionShape(p.GEOM_BOX,halfExtents=[sphereRadius,sphereRadius,sphereRadius])
|
||||
|
||||
mass = 1
|
||||
visualShapeId = -1
|
||||
|
||||
|
||||
|
||||
link_Masses=[1]
|
||||
linkCollisionShapeIndices=[colBoxId]
|
||||
linkVisualShapeIndices=[-1]
|
||||
linkPositions=[[0,0,0.11]]
|
||||
linkOrientations=[[0,0,0,1]]
|
||||
linkInertialFramePositions=[[0,0,0]]
|
||||
linkInertialFrameOrientations=[[0,0,0,1]]
|
||||
indices=[0]
|
||||
jointTypes=[p.JOINT_REVOLUTE]
|
||||
axis=[[0,0,1]]
|
||||
|
||||
allSpheres = []
|
||||
|
||||
|
||||
framePerSec = 13
|
||||
|
||||
r_physics = PhysRenderer(opt.datapath, opt.model, framePerSec)
|
||||
|
||||
#objectUid = p.loadURDF("models/quadrotor.urdf", globalScaling = 0.8)
|
||||
objectUid = p.loadURDF("models/husky.urdf", globalScaling = 0.8)
|
||||
#p.changeVisualShape(objectUid, -1, rgbaColor=[1, 1, 1, 0.5])
|
||||
|
||||
pos, quat_xyzw = getInitialPositionOrientation()
|
||||
|
||||
v_t = 1 # 1m/s max speed
|
||||
v_r = np.pi/5 # 36 degrees/s
|
||||
#pos = [0, 0, 3]
|
||||
#quat_xyzw = [0, 0, 0, 3]
|
||||
cart = PhysicsObject(objectUid, p, pos, quat_xyzw, v_t, v_r, framePerSec)
|
||||
|
||||
#pos, quat = p.getViewPosAndOrientation(cart)
|
||||
#p.resetBasePositionAndOrientation(cart, [pos[0], pos[1], 1], quat)
|
||||
|
||||
print("Generated cart", objectUid)
|
||||
|
||||
p.setGravity(0,0,-10)
|
||||
p.setRealTimeSimulation(0)
|
||||
|
||||
## same as cv.waitKey(5) in viewPort
|
||||
|
||||
|
||||
#p.setTimeStep(1.0/framePerSec)
|
||||
p.setTimeStep(1.0/settings.STEPS_PER_SEC)
|
||||
|
||||
startttime = time.time()
|
||||
lasttime = time.time()
|
||||
while (1):
|
||||
## Execute one frame
|
||||
cart.getUpdateFromKeyboard()
|
||||
sendPoseToViewPort(cart.getViewPosAndOrientation())
|
||||
#time.sleep(1.0/framePerSec)
|
||||
#p.stepSimulation()
|
||||
simutime = time.time()
|
||||
print('time step', 1.0/settings.STEPS_PER_SEC, 'stepping', settings.STEPS_PER_SEC/framePerSec)
|
||||
stepNsteps(int(settings.STEPS_PER_SEC/framePerSec), cart)
|
||||
#for _ in range(int(settings.STEPS_PER_SEC/framePerSec)):
|
||||
# p.stepSimulation()
|
||||
|
||||
print("passed time", time.time() - lasttime, "simulation time", time.time() - simutime)
|
||||
lasttime = time.time()
|
||||
print("last time", lasttime, "start time", startttime)
|
||||
if lasttime - startttime > 5:
|
||||
startttime = lasttime
|
||||
cart.getUpdateFromKeyboard(restart=True)
|
||||
stepNsteps(int(settings.STEPS_PER_SEC/framePerSec), cart)
|
||||
|
||||
#if PHYSICS_FIRST:
|
||||
## Physics-first simulation
|
||||
#synchronizeWithViewPort()
|
||||
#p.stepSimulation()
|
||||
#time.sleep(0.05)
|
||||
|
||||
#else:
|
||||
## Visual-first simulation
|
||||
#getCollisionFromUpdate()
|
||||
r_physics.renderToScreen()
|
||||
|
||||
|
|
@ -6,6 +6,7 @@ from render.datasets import ViewDataSet3D
|
|||
from render.show_3d2 import PCRenderer, sync_coords
|
||||
import numpy as np
|
||||
import zmq
|
||||
import time
|
||||
|
||||
class SimpleEnv(gym.Env):
|
||||
metadata = {'render.modes': ['human']}
|
||||
|
|
@ -14,22 +15,22 @@ class SimpleEnv(gym.Env):
|
|||
cmd_channel = "./channels/depth_render/depth_render --datapath data -m 11HB6XZSh1Q"
|
||||
cmd_physics = "python show_3d2.py --datapath ../data/ --idx 10"
|
||||
cmd_render = ""
|
||||
self.datapath = "data"
|
||||
self.model_id = "11HB6XZSh1Q"
|
||||
#self.p_channel = subprocess.Popen(cmd_channel.split(), stdout=subprocess.PIPE)
|
||||
#self.p_physics = subprocess.Popen()
|
||||
#self.p_render = subprocess.Popen()
|
||||
self.r_visuals = self._setupRenderer()
|
||||
#self.r_physics = self._setupPhysics()
|
||||
|
||||
self.p_renderer = self._setupRenderer()
|
||||
|
||||
|
||||
def _setupRenderer(self):
|
||||
datapath = "data"
|
||||
model_id = "11HB6XZSh1Q"
|
||||
d = ViewDataSet3D(root=datapath, transform = np.array, mist_transform = np.array, seqlen = 2, off_3d = False, train = False)
|
||||
d = ViewDataSet3D(root=self.datapath, transform = np.array, mist_transform = np.array, seqlen = 2, off_3d = False, train = False)
|
||||
|
||||
scene_dict = dict(zip(d.scenes, range(len(d.scenes))))
|
||||
if not model_id in scene_dict.keys():
|
||||
if not self.model_id in scene_dict.keys():
|
||||
print("model not found")
|
||||
else:
|
||||
scene_id = scene_dict[model_id]
|
||||
scene_id = scene_dict[self.model_id]
|
||||
uuids, rts = d.get_scene_info(scene_id)
|
||||
#print(uuids, rts)
|
||||
targets = []
|
||||
|
|
@ -69,7 +70,7 @@ class SimpleEnv(gym.Env):
|
|||
|
||||
def _step(self, action):
|
||||
#renderer.renderToScreen(sources, source_depths, poses, model, target, target_depth, rts)
|
||||
print(self.p_renderer.renderOffScreen().size)
|
||||
print(self.r_visuals.renderOffScreen().size)
|
||||
return
|
||||
|
||||
def _reset(self):
|
||||
|
|
@ -81,11 +82,15 @@ class SimpleEnv(gym.Env):
|
|||
def _end(self):
|
||||
#self.p_channel.kill()
|
||||
#self.p_physics.kill()
|
||||
#self.p_render.kill()
|
||||
#self.r_visuals.kill()
|
||||
return
|
||||
|
||||
|
||||
if __name__ == "__main__":
|
||||
env = SimpleEnv()
|
||||
while True:
|
||||
env._step({})
|
||||
t0 = time.time()
|
||||
env._step({})
|
||||
t1 = time.time()
|
||||
t = t1-t0
|
||||
print('fps', 1/t)
|
||||
Loading…
Reference in New Issue