Files

20 lines
1.1 KiB
Python

"""Replay offline approach or contact dynamics. Space=pause, R=restart; mouse=orbit."""
import argparse,time
from pathlib import Path
import numpy as np
import mujoco
import mujoco.viewer
p=argparse.ArgumentParser();p.add_argument('--dynamic',action='store_true');p.add_argument('--middle',action='store_true');a=p.parse_args()
r=Path(__file__).resolve().parents[1];folder=r/('data/cup_middle_search' if a.middle else 'data/cup_approach_search')
m=mujoco.MjModel.from_xml_path(str(folder/'dynamic_scene.xml' if a.dynamic else r/'scene/cup_workcell.xml'));d=mujoco.MjData(m)
frames=np.load(folder/('dynamic_qpos.npy' if a.dynamic else 'approach_qpos.npy'));state={'pause':False,'i':0}
def key(k):
if k==32:state['pause']=not state['pause']
if k in (82,114):state['i']=0
with mujoco.viewer.launch_passive(m,d,key_callback=key) as v:
v.cam.lookat[:]=[.27,0,.90];v.cam.distance=1.2;v.cam.azimuth=135;v.cam.elevation=-20
while v.is_running():
d.qpos[:]=frames[state['i']];mujoco.mj_forward(m,d);v.sync()
if not state['pause']:state['i']=min(state['i']+1,len(frames)-1)
time.sleep(.02)