def gtgen(gtmapfn, base, nsample, outdir):
assert nsample <= 999999, 'Do not support 7+ digits of samples for now'
if not os.path.exists(outdir):
os.makedirs(outdir, mode=0770)
uw = pyosr.UnitWorld()
uw.loadModelFromFile(aniconf.env_fn)
uw.loadRobotFromFile(aniconf.rob_fn)
uw.scaleToUnit()
uw.angleModel(0.0, 0.0)
'''
init_state = np.array([17.97,7.23,10.2,1.0,0.0,0.0,0.0])
if not uw.is_valid_state(uw.translate_to_unit_state(init_state)):
return
'''
gt = pyosr.GTGenerator(uw)
gt.rl_stepping_size = 0.0125
gt.verify_magnitude = 0.0125 / 64
gtdata = np.load(gtmapfn)
gt.install_gtdata(gtdata['V'], gtdata['E'], gtdata['D'], gtdata['N'])
gt.init_knn_in_batch()
for i in range(nsample):
ofn = "{}/aa-gt-{:06d}.npz".format(outdir, base + i)
if os.path.exists(ofn):
print("Skipping existing file {}".format(ofn))
continue
is_final = False
while not is_final:
init_state = uw_random.gen_init_state(uw)
keys, _, is_final = gt.generate_gt_path(init_state, 1024 * 4, False)
cont_tr, cont_rot, cont_dists, _ = gt.cast_path_to_cont_actions_in_UW(keys, path_is_verified=True)
np.savez(ofn, TR=cont_tr, ROT=cont_rot, DIST=cont_dists)
def verifymap(mapfn):
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
r.setup()
r.loadModelFromFile(aniconf.env_fn)
r.loadRobotFromFile(aniconf.rob_fn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
'''
st0 = np.array([21.782108575648873,11.070742691783639,13.072090341969885,0.99496368307688909,-0.050573680994590003,0.08255004745739393,0.025981951687884433], dtype=np.float64)
st0 = r.translate_to_unit_state(st0.transpose())
st1 = np.array([24.404447383193428,16.614281021136808,17.241012748680941,0.89856334412742611,-0.42392368380753659,0.035352511370216902,0.10780921499298371], dtype=np.float64)
st1 = r.translate_to_unit_state(st1.transpose())
print(r.transit_state_to(st0, st1, 0.00125))
return
'''
gt = pyosr.GTGenerator(r)
gt.rl_stepping_size = 0.0125
# gt.verify_magnitude = 0.0125
gt.load_roadmap_file(mapfn)
# gt.init_gt()
gt.save_verified_roadmap_file(mapfn + ".verified")
return
def gtgen(gtfn, gtdir, gtnumber):
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
r.setup()
r.loadModelFromFile(aniconf.env_fn)
r.loadRobotFromFile(aniconf.rob_fn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
gt = pyosr.GTGenerator(r)
gt.rl_stepping_size = 0.0125
gtdata = np.load(gtfn)
gt.install_gtdata(gtdata['V'], gtdata['E'], gtdata['D'], gtdata['N'])
gt.init_knn_in_batch()
print(r.scene_matrix)
print(r.robot_matrix)
# return
for i in range(gtnumber):
# Random state, copied from RLDriver.restart_epoch
while True:
tr = np.random.rand(3) * 2.0 - 1.0
u1, u2, u3 = np.random.rand(3)
quat = [
sqrt(1 - u1) * sin(2 * pi * u2),
sqrt(1 - u1) * cos(2 * pi * u2),
sqrt(u1) * sin(2 * pi * u3),
sqrt(u1) * cos(2 * pi * u3)
]
part1 = np.array(tr, dtype=np.float32)
part2 = np.array(quat, dtype=np.float32)
r.state = np.concatenate((part1, part2))
if r.is_disentangled(r.state):
continue
if r.is_valid_state(r.state):
break
gtstats, gtactions, terminated = gt.generate_gt_path(
r.state, MAX_GT_STATES)
if not terminated:
'''
Try again
'''
continue
def reanimate(gtfn, fn):
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
r.setup()
r.loadModelFromFile(aniconf.env_fn)
r.loadRobotFromFile(aniconf.rob_fn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
gt = pyosr.GTGenerator(r)
gt.rl_stepping_size = 0.0125 / 16
gt.verify_magnitude = 0.0125 / 16 / 8
# gt.rl_stepping_size = 0.0125 / 16 / 1024
# gt.verify_magnitude = 0.0125 / 16 / 1024 / 2
# gt.rl_stepping_size = 0.0125 / 64
# gt.verify_magnitude = 0.0125 / 64 / 8
gtdata = np.load(gtfn)
gt.install_gtdata(gtdata['V'], gtdata['E'], gtdata['D'], gtdata['N'])
# FIXME: add this back after debugging Dijkstra
# gt.init_knn_in_batch()
class ReAnimator(object):
reaching_terminal = False
driver = None
im = None
cont_actions = None
def __init__(self,
renderer,
init_state,
cont_tr,
cont_rot,
delta=0.125):
self.renderer = renderer
self.cont_tr = cont_tr
self.cont_rot = cont_rot
self.t = 0.0
assert delta <= 1.0, "delta must be <= 1.0, we are reanimating with stateful ACTIONS"
self.delta = delta
self.state = renderer.translate_to_unit_state(init_state)
self.reaching_terminal = False
def perform(self, framedata):
r = self.renderer
index = int(math.floor(self.t))
if index < len(self.cont_tr) and not self.reaching_terminal:
pkey = self.state
nkey, _, _ = r.transit_state_by(pkey, self.cont_tr[index],
self.cont_rot[index],
gt.verify_magnitude)
tau = self.t - index
print("tau {}".format(tau))
state = interpolate(pkey, nkey, tau)
r.state = state
r.render_mvrgbd()
print(r.mvrgb.shape)
rgb = r.mvrgb.reshape((r.pbufferWidth, r.pbufferHeight, 3))
print(r.state)
valid = r.is_valid_state(r.state)
disentangled = r.is_disentangled(r.state)
print('\tNew State {} Collision Free ? {} Disentangled ? {}'.
format(r.state, valid, disentangled))
if not valid:
print('\tNOT COLLISION FREE, SAN CHECK FAILED')
self.reaching_terminal = True
if self.im is None:
print('rgb {}'.format(rgb.shape))
self.im = plt.imshow(rgb)
else:
self.im.set_array(rgb)
self.t += self.delta
if index != int(math.floor(self.t)):
self.state = nkey
fig = plt.figure()
# fn = 'blend-cont-saved.npz'
if not os.path.isfile(fn):
# This is a legacy debugging code path
print("Translating into Cont. Actions...")
cont_tr, cont_rot, _, _ = gt.cast_path_to_cont_actions_in_UW(keys)
print("Done")
np.savez('blend-cont.npz', TR=cont_tr, ROT=cont_rot)
keys = np.loadtxt('blend.path')
else:
print("Loading cacahed Cont. Actions")
dic = np.load(fn)
cont_tr = dic['TR']
cont_rot = dic['ROT']
keys = dic['KEYS']
# keys[:, [3,4,5,6]] = keys[:,[6,3,4,5]]
ra = ReAnimator(r, keys[0], cont_tr, cont_rot, 1.0)
ani = animation.FuncAnimation(fig, ra.perform)
plt.show()
def reanimate(gtfn, anifn):
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
r.setup()
r.loadModelFromFile(aniconf.env_fn)
r.loadRobotFromFile(aniconf.rob_fn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
gt = pyosr.GTGenerator(r)
# gt.rl_stepping_size = 0.0125 / 64 # it's for animation only if AA is enabled
gt.verify_magnitude = 0.0125 / 64 / 8
# print('default rl_stepping_size {}'.format(gt.rl_stepping_size))
gtdata = np.load(gtfn)
gt.install_gtdata(gtdata['V'], gtdata['E'], gtdata['D'], gtdata['N'])
# FIXME: add this back after debugging Dijkstra
# gt.init_knn_in_batch()
print(r.scene_matrix)
print(r.robot_matrix)
# return
class ReAnimator(object):
reaching_terminal = False
driver = None
im = None
keys = None
prev_state = None
def __init__(self, renderer, keys, delta=0.125):
self.renderer = renderer
self.keys = keys
self.t = 0.0
self.delta = delta
self.reaching_terminal = False
self.prev_state = self.renderer.translate_to_unit_state(keys[0])
def perform(self, framedata):
r = self.renderer
index = int(math.floor(self.t))
nindex = index + 1
if nindex < len(self.keys) and not self.reaching_terminal:
pkey = self.keys[index]
nkey = self.keys[nindex]
tau = self.t - index
print("tau {}".format(tau))
state = interpolate(pkey, nkey, tau)
r.state = r.translate_to_unit_state(state)
r.render_mvrgbd()
# print(r.mvrgb.shape)
rgb = r.mvrgb.reshape((r.pbufferWidth, r.pbufferHeight, 3))
# print(r.state)
valid = r.is_valid_state(r.state)
print('\tNew State {} Collision Free ? {}'.format(
r.state, valid))
print('\tDistance {}'.format(
pyosr.distance(r.state, self.prev_state)))
if not valid:
print(
'\tNOT COLLISION FREE, SAN CHECK FAILED AT {}'.format(
self.t))
self.reaching_terminal = True
if self.im is None:
print('rgb {}'.format(rgb.shape))
self.im = plt.imshow(rgb)
else:
self.im.set_array(rgb)
self.prev_state = r.state
self.t += self.delta
fig = plt.figure()
keys = np.loadtxt(aniconf.keys_fn)
keys[:, [3, 4, 5, 6]] = keys[:, [6, 3, 4, 5]]
init_state = keys[0]
# init_state = np.array([0.36937377,0.1908864,0.21092376, 0.98570921, -0.12078825, -0.0675864, 0.09601888], dtype=np.float64)
# gt.init_knn_in_batch()
if anifn is None:
STEP_LIMIT = 64 * 1024
FOR_RL = False
gtstats, actions, terminated = gt.generate_gt_path(
init_state, STEP_LIMIT, FOR_RL)
if STEP_LIMIT >= 1024:
np.savez('data-from-init_state', S=gtstats, A=actions)
return
print("terminated ? {} Expecting True".format(terminated))
else:
'''
gtdata = np.load(anifn)
gtstats = gtdata['S']
'''
gtstats = np.loadtxt(anifn)
# gtstats[:, [3,4,5,6]] = gtstats[:,[6,3,4,5]]
print(gtstats.shape)
# print('trajectory:\n')
# print(gtstats)
ra = ReAnimator(r, gtstats, 0.05)
ani = animation.FuncAnimation(fig, ra.perform)
plt.show()
def reanimate(gtfn, pathfn, swap, cachedir, interpthresh=0, in_unit=True):
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
r.avi = True
r.setup()
r.loadModelFromFile(aniconf.env_fn)
r.loadRobotFromFile(aniconf.rob_fn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
gt = pyosr.GTGenerator(r)
# gt.rl_stepping_size = 0.0125 / 16
# gt.verify_magnitude = 0.0125 / 16 / 8
# gt.rl_stepping_size = 0.0125 / 16 / 1024
# gt.verify_magnitude = 0.0125 / 16 / 1024 / 2
gt.rl_stepping_size = 0.0125 * 4
gt.verify_magnitude = 0.0125 * 4 / 8
# gt.rl_stepping_size = 0.0125 / 64
# gt.verify_magnitude = 0.0125 / 64 / 8
gtdata = np.load(gtfn)
gt.install_gtdata(gtdata['V'], gtdata['E'], gtdata['D'], gtdata['N'])
# FIXME: add this back after debugging Dijkstra
# gt.init_knn_in_batch()
class ExpandingReAnimator(object):
reaching_terminal = False
driver = None
im = None
keys = None
prev_state = None
gt = None
state = None
def __init__(self, renderer, gt, keys, delta=1):
self.renderer = renderer
self.gt = gt
self.keys = keys
self.t = 0.0
self.delta = delta
self.reaching_terminal = False
self.prev_state = self.renderer.translate_to_unit_state(keys[0])
self.st_index = 0
self.state = self.keys[0]
self.expand_states()
def expand_states(self):
keys = self.keys
if self.st_index + 1 >= keys.shape[0]:
self.reaching_terminal = True
return
r = self.renderer
# r.light_position = np.random.rand(3)
'''
from_state = r.translate_to_unit_state(keys[self.st_index])
to_state = r.translate_to_unit_state(keys[self.st_index + 1])
print(from_state)
print(to_state)
mag = pyosr.distance(from_state, to_state)
'''
'''
for a in range(12):
tup = r.transit_state(from_state, a, gt.rl_stepping_size, gt.verify_magnitude)
print(tup)
print("distance from start: {}".format(pyosr.distance(tup[0], from_state)))
d = pyosr.distance(tup[0], to_state)
print("distance to next: {} Closer? {}".format(d, d < mag))
return
'''
fn = '{}/{}.npz'.format(cachedir, self.st_index)
if not os.path.isfile(fn):
tup = self.gt.project_trajectory(self.state,
keys[self.st_index + 1],
in_unit=in_unit)
self.exp_st = tup[0]
if self.exp_st.shape[0] > 0:
self.state = self.exp_st[-1]
print("cache states to {}".format(fn))
np.savez(fn, S=tup[0], A=tup[1])
else:
print("load cached states from {}".format(fn))
cache = np.load(fn)['S']
self.exp_st = cache
# print(self.exp_st)
self.exp_st_index = 0
self.st_index += 1
def perform(self, framedata):
r = self.renderer
while self.exp_st_index >= self.exp_st.shape[0]:
self.expand_states()
if self.reaching_terminal == True:
print("Reaching Terminal")
return
'''
if self.exp_st_index > self.keys.shape[0]:
return
r.state = r.translate_to_unit_state(self.keys[self.exp_st_index])
'''
r.state = r.translate_to_unit_state(self.exp_st[self.exp_st_index])
r.render_mvrgbd()
rgb = r.mvrgb.reshape((r.pbufferWidth, r.pbufferHeight, 3))
if self.im is None:
self.im = plt.imshow(rgb)
else:
self.im.set_array(rgb)
self.prev_state = r.state
self.exp_st_index += self.delta
print('{}: {}'.format(self.exp_st_index, r.state))
'''
index = int(math.floor(self.t))
nindex = index + 1
if nindex < len(self.keys) and not self.reaching_terminal:
pkey = self.keys[index]
nkey = self.keys[nindex]
tau = self.t - index
print("tau {}".format(tau))
state = interpolate(pkey, nkey, tau)
r.state = r.translate_to_unit_state(state)
r.render_mvrgbd()
# print(r.mvrgb.shape)
rgb = r.mvrgb.reshape((r.pbufferWidth, r.pbufferHeight, 3))
# print(r.state)
valid = r.is_valid_state(r.state)
print('\tNew State {} Collision Free ? {}'.format(r.state, valid))
print('\tDistance {}'.format(pyosr.distance(r.state, self.prev_state)))
if not valid:
print('\tNOT COLLISION FREE, SAN CHECK FAILED AT {}'.format(self.t))
self.reaching_terminal = True
if self.im is None:
print('rgb {}'.format(rgb.shape))
self.im = plt.imshow(rgb)
else:
self.im.set_array(rgb)
self.prev_state = r.state
self.t += self.delta
'''
fig = plt.figure()
keys = np.loadtxt(pathfn)
if swap:
'''
Transpose W-Last data into W-First format
'''
keys[:, [3, 4, 5, 6]] = keys[:, [6, 3, 4, 5]]
if interpthresh > 0:
ikeys = [keys[0]]
for key in keys[1:]:
print("Partition {}\n{}".format(ikeys[-1], key))
inodes = partition(ikeys[-1], key, interpthresh)
ikeys += inodes
ikeys.append(key)
ikeys = np.array(ikeys)
print("Using: {}".format(ikeys))
print("Total nodes: {}".format(len(ikeys)))
ra = ExpandingReAnimator(r, gt, keys)
ani = animation.FuncAnimation(fig, ra.perform)
plt.show()