def create_renderer(args, creating_ctx=True):
view_num, view_array = get_view_cfg(args)
w = h = args.res
if creating_ctx:
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
if args.avi:
r.avi = True
r.pbufferWidth = w
r.pbufferHeight = h
r.setup()
r.loadModelFromFile(args.envgeo)
r.loadRobotFromFile(args.robgeo)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
if args.robcenter is not None:
r.enforceRobotCenter(args.robcenter)
if args.view >= 0:
if args.obview >= 0:
va = [view_array[args.obview]]
else:
va = [view_array[args.view]]
else:
va = view_array
r.views = np.array(va, dtype=np.float32)
return r
def render_everything_to(src, dst, label):
pyosr.init()
pyosr.create_gl_context(pyosr.create_display())
modelId = 0
r=pyosr.Renderer()
r.setup()
r.default_depth = 0.0
view_array = []
for angle,ncam in config.VIEW_CFG:
view_array += [ [angle,float(i)] for i in np.arange(0.0, 360.0, 360.0/float(ncam)) ]
r.views = np.array(view_array)
w = r.pbufferWidth
h = r.pbufferHeight
for root, dirs, files in os.walk(src):
for fn in files:
# print(fn)
_,ext = os.path.splitext(fn)
if ext not in ['.dae', '.obj', '.ply']:
continue
ffn = os.path.join(root, fn)
r.loadModelFromFile(ffn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
dep = r.render_mvdepth_to_buffer()
dep = dep.reshape(r.views.shape[0],w,h,1)
outfn = os.path.join(dst, '%07d.train' % modelId)
with open(outfn, 'w') as f:
if label >= 0:
labelbytes = np.array([label],dtype=np.int32).tobytes()
f.write(labelbytes)
f.write(dep.tobytes())
print('{} -> {}'.format(ffn, outfn))
modelId += 1
def __init__(self, env, rob, center=None, res=224, flat_surface=False):
pyosr.init()
self.dpy = pyosr.create_display()
self.glctx = pyosr.create_gl_context(self.dpy)
r = pyosr.Renderer()
r.avi = True
r.flat_surface = flat_surface
r.pbufferWidth = res
r.pbufferHeight = res
r.setup()
r.loadModelFromFile(env)
r.loadRobotFromFile(rob)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
if center is not None:
r.enforceRobotCenter(center)
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
self.r = r
self.res = res
self.rgb_shape = (res, res, 3)
self.dep_shape = (res, res, 1)
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 test():
pyosr.init()
pyosr.create_gl_context(pyosr.create_display())
r=pyosr.Renderer()
r.setup()
r.loadModelFromFile('../res/alpha/env-1.2.obj')
r.loadRobotFromFile('../res/alpha/robot.obj')
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[30.0, float(i)] for i in range(0, 360, 30)], dtype=np.float32)
print(r.views)
print(r.views.shape)
w = r.pbufferWidth
h = r.pbufferHeight
r.state = np.array([0.2, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5], dtype=np.float32)
r.render_mvrgbd()
img = r.mvrgb.reshape(w * r.views.shape[0], h, 3)
dep = r.mvdepth.reshape(w * r.views.shape[0], h)
depimg = Image.fromarray(dep)
imsave('mvrgb.png', img)
depimg.save('mvdepth.tiff')
# Vision NN
vis0 = vision.VisionLayerConfig(16)
vis1 = vision.VisionLayerConfig(16)
vis1.strides = [1, 3, 3, 1]
vis1.kernel_size = [5, 5]
vis2 = vision.VisionLayerConfig(32)
vis3 = vision.VisionLayerConfig(64)
imgin = img.reshape(r.views.shape[0], w, h, 3)
depin = r.mvdepth.reshape(r.views.shape[0], w , h, 1)
print('imgin shape: {}'.format(imgin.shape))
mv_color = vision.VisionNetwork(imgin.shape,
[vis0, vis1, vis2, vis3], 0, 256)
featvec = mv_color.features
print('mv_color.featvec.shape = {}'.format(featvec.shape))
sq_featvec = tf.reshape(featvec, [-1, 16, 16, 1]) # Squared feature vector
chmv_featvec = tf.transpose(sq_featvec, [3, 1, 2, 0])
print('chmv_featvec {}'.format(chmv_featvec.shape))
pvis0 = vision.VisionLayerConfig(64)
color = vision.VisionNetwork(None, [pvis0], 0, 256, chmv_featvec)
featvec = color.features
print('featvec.shape = {}'.format(featvec.shape))
exit()
mvpix = r.render_mvdepth_to_buffer()
img = mvpix.reshape(w * r.views.shape[0], h)
print(img.shape)
plt.pcolor(img)
plt.show()
r.teardown()
pyosr.shutdown()
def _create_renderer(args):
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
if hasattr(args, '_fb_resolution'):
r.pbufferWidth = args._fb_resolution
r.pbufferHeight = args._fb_resolution
r.setup()
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
return r
def test():
pyosr.init()
pyosr.create_gl_context(pyosr.create_display())
r = pyosr.Renderer()
r.setup()
r.loadModelFromFile('../res/alpha/env-1.2.obj')
r.loadRobotFromFile('../res/alpha/robot.obj')
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
r.views = np.array([[30.0, float(i)] for i in range(0, 360, 30)],
dtype=np.float32)
print(r.views)
print(r.views.shape)
w = r.pbufferWidth
h = r.pbufferHeight
r.state = np.array([0.125, -0.075, 0.0, 0.5, 0.5, 0.5, 0.5],
dtype=np.float32)
r.render_mvrgbd()
img = r.mvrgb.reshape(w * r.views.shape[0], h, 3)
dep = r.mvdepth.reshape(w * r.views.shape[0], h)
depimg = Image.fromarray(dep)
imsave('mvrgb.png', img)
depimg.save('mvdepth.tiff')
print("Is colliding free? {} {}".format(r.state,
r.is_valid_state(r.state)))
r.state = np.array([0.04, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5], dtype=np.float32)
print("Is colliding free? {} {} Expecting True".format(
r.state, r.is_valid_state(r.state)))
r.state = np.array([0.7, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5], dtype=np.float32)
print("Is colliding free? {} {} Expecting True".format(
r.state, r.is_valid_state(r.state)))
goal_state = np.array([0.04, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5],
dtype=np.float32)
# mags = np.array([0.7 - 0.04, 0.125], dtype=np.float32)
# deltas = np.array([0.0125, 0.0125], dtype=np.float32)
mags = 0.7 - 0.04
deltas = 0.0125
print(r.transit_state(r.state, 1, mags, deltas))
print(r.transit_state(r.state, 6, mags, deltas))
for x in np.arange(-0.8, 0.8, 0.02):
r.state = np.array([x, 0.0, 0.0, 0.5, 0.5, 0.5, 0.5], dtype=np.float32)
print("Is colliding free? {} {}".format(r.state,
r.is_valid_state(r.state)))
r.render_mvrgbd()
# print('r.mvrgb {}'.format(r.mvrgb.shape))
# print('r.views {} w {} h {}'.format(r.views.shape, w, h))
img = r.mvrgb.reshape(w * r.views.shape[0], h, 3)
imsave('mvrgb-x={}.png'.format(x), img)
r.teardown()
pyosr.shutdown()
def _create_r():
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)
return r
def create_offscreen_renderer(puzzle_file, resolution=256):
global _egl_dpy
import pyosr
if _egl_dpy is None:
pyosr.init()
_egl_dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(_egl_dpy)
r = pyosr.Renderer()
r.pbufferWidth = resolution
r.pbufferHeight = resolution
r.setup()
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
_load_unit_world(r, puzzle_file)
return r
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 __init__(self,
env,
rob,
rob_texfn='',
center=None,
res=256,
flat_surface=False,
gen_surface_normal=False):
if self.dpy is None:
pyosr.init()
self.dpy = pyosr.create_display()
self.glctx = pyosr.create_gl_context(self.dpy)
self.env_fn = env
self.rob_fn = rob
self.name = 'Unnamed'
r = pyosr.Renderer()
r.avi = True
r.flat_surface = flat_surface
r.pbufferWidth = res
r.pbufferHeight = res
r.setup()
r.loadModelFromFile(env)
r.loadRobotFromFile(rob)
if rob_texfn:
r.loadRobotTextureImage(rob_texfn)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
if center is not None:
r.enforceRobotCenter(center)
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
self.r = r
self.res = res
self.rgb_shape = (res, res, 3)
self.dep_shape = (res, res, 1)
self.gen_surface_normal = gen_surface_normal
def _create_uw(aniconf, cmd):
if cmd == 'show':
return None # show command does not need unit world object
if 'render' in cmd or cmd in ['atlas2prim', 'useuniformatlas']:
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer() # 'project' command requires a Renderer
if cmd in ['atlas2prim', 'useuniformatlas']:
r.pbufferWidth = ATLAS_RES
r.pbufferHeight = ATLAS_RES
r.setup()
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
else:
r = pyosr.UnitWorld() # pyosr.Renderer is not avaliable in HTCondor
if aniconf.rob_ompl_center is None:
'''
When OMPL_center is not specified
'''
assert aniconf.env_wt_fn == aniconf.env_uv_fn
assert aniconf.rob_wt_fn == aniconf.rob_uv_fn
if cmd in ['run', 'isect']:
# fb = r.render_barycentric(r.BARY_RENDERING_ROBOT, np.array([1024, 1024], dtype=np.int32))
# imsave('1.png', fb)
# sys.exit(0)
r.loadModelFromFile(aniconf.env_wt_fn)
r.loadRobotFromFile(aniconf.rob_wt_fn)
else: # All the remaining commands need UV coordinates
r.loadModelFromFile(aniconf.env_uv_fn)
r.loadRobotFromFile(aniconf.rob_uv_fn)
if aniconf.rob_ompl_center is not None:
r.enforceRobotCenter(aniconf.rob_ompl_center)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
return r
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()
import pyosr
import numpy as np
import matplotlib.pyplot as plt
pyosr.init()
pyosr.create_gl_context(pyosr.create_display())
r = pyosr.Renderer()
r.setup()
r.loadModelFromFile('../res/alpha/robot.obj')
r.angleModel(30.0, 30.0)
pix = r.render_depth_to_buffer()
r.teardown()
pyosr.shutdown()
w = r.pbufferWidth
h = r.pbufferHeight
img = np.array(pix, dtype=np.float32)
img = img.reshape(w, h)
plt.pcolor(img)
plt.show()
def calibrate(gtfn, cfn):
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)
origin = np.array([0,0,0,1,0,0,0], dtype=np.float64)
d = np.load(gtfn)
V=d['V']
D=d['D']
N=d['N']
pos = []
posd = []
neg = []
negd = []
for i in range(len(V)):
V[i] = r.translate_to_unit_state(V[i])
if r.is_disentangled(V[i]):
D[i] = 0.0
assert len(d['N']) == len(V), "N size ({}) does not match V's ({})".format(len(d['N']), len(V))
print("istate {} distance {}".format(V[0], pyosr.distance(origin, V[0])))
print("gstate {} distance {}".format(V[1], pyosr.distance(origin, V[1])))
# Trim distant samples, which usually shows nothing
old_to_new = {}
new_to_old = {}
NN = []
for i in range(len(V)):
if pyosr.distance(origin, V[i]) > 0.85:
continue
old_to_new[i] = len(pos)
pos.append(V[i])
posd.append(D[i])
for i in range(len(V)):
if pyosr.distance(origin, V[i]) > 0.85:
continue
if N[i] < 0:
NN.append(N[i])
else:
NN.append(old_to_new[N[i]])
NE = []
for e in d['E']:
if e[0] not in old_to_new or e[1] not in old_to_new:
continue
NE.append([old_to_new[e[0]], old_to_new[e[1]]])
assert len(NN) == len(pos), "NN size ({}) does not match pos' ({})".format(len(NN), len(pos))
V = np.array(pos)
D = np.array(posd)
for i in range(len(V)):
while True:
s = uw_random.random_state()
if not r.is_disentangled(s):
break
neg.append(s)
negd.append(-10.0)
NN.append(-1)
NV = np.array(neg)
ND = np.array(negd)
V = np.concatenate((V, NV))
D = np.concatenate((D, ND))
E = np.array(NE)
N = np.array(NN)
#p = np.random.permutation(len(V))
#np.savez(cfn, V=V[p], E=E, D=D[p], N=N[p])
np.savez(cfn, V=V, E=E, D=D, N=N)
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 __init__(
self,
models,
init_state,
view_config,
svconfdict,
mvconfdict,
# output_number should be
# For discrete action RL: X,Y,Z * (rotate,translate) * (pos,neg)
# For continuous action RL: (X,Y,Z).(AA-X,AA-Y,AA-Z)
output_number=3 * 2 * 2,
sv_sqfeatnum=16,
mv_featnum=256,
input_tensor=None,
use_rgb=False,
master_driver=None,
grads_applier=None,
worker_thread_index=-1,
continuous_policy_loss=False):
self.init_state = init_state
self.worker_thread_index = worker_thread_index
self.grads_applier = grads_applier
self.action_size = output_number
self.continuous_policy_loss = continuous_policy_loss
self.renderer = pyosr.Renderer()
r = self.renderer
r.pbufferWidth = config.DEFAULT_RES
r.pbufferHeight = config.DEFAULT_RES
if master_driver is None:
r.setup()
r.loadModelFromFile(models[0])
if len(models) > 1 and models[1] is not None:
r.loadRobotFromFile(models[1])
r.state = np.array(init_state, dtype=np.float32)
print('robot loaded')
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
else:
r.setupFrom(master_driver.renderer)
r.default_depth = master_driver.renderer.default_depth
r.state = np.array(init_state, dtype=np.float32)
self.master = master_driver
view_array = []
for angle, ncam in view_config:
view_array += [[angle, float(i)]
for i in np.arange(0.0, 360.0, 360.0 / float(ncam))]
r.views = np.array(view_array, dtype=np.float32)
w = r.pbufferWidth
h = r.pbufferHeight
# TODO: Switch to RGB-D rather than D-only
CHANNEL = 1
inputshape = [None, len(view_array), w, h, CHANNEL
] if input_tensor is None else None
self.sv_depth_net, sv_depth_featvec = self._create_sv_features(
inputshape, input_tensor, svconfdict, len(view_array),
sv_sqfeatnum)
self.sv_depth_shape = [s if s is not None else -1 for s in inputshape]
print('sv_depth_featvec: {}'.format(sv_depth_featvec.shape))
if use_rgb is True:
CHANNEL = 3
# input_tensor is always for depth
inputshape = [None, len(view_array), w, h, CHANNEL]
self.sv_rgb_net, sv_rgb_featvec = self._create_sv_features(
inputshape, None, svconfdict, len(view_array), sv_sqfeatnum)
self.sv_rgb_shape = [
s if s is not None else -1 for s in inputshape
]
# Concat B1WHV and B1WHV into B1WHV
print('sv_rgb_featvec: {}'.format(sv_rgb_featvec.shape))
sv_featvec = tf.concat([sv_depth_featvec, sv_rgb_featvec], 4)
else:
self.sv_rgb_net = None
sv_rgb_featvec = None
sv_featvec = sv_depth_featvec
print('sv_featvec: {}'.format(sv_featvec.shape))
mv_net = vision.VisionNetwork(
None,
vision.VisionLayerConfig.createFromDict(mvconfdict),
0, # FIXME: multi-threading
mv_featnum,
sv_featvec)
self.mv_net = mv_net
self.sv_depthfv = sv_depth_featvec
self.sv_rgbfv = sv_rgb_featvec
self.mv_fv = mv_net.features
conf_final_fc = vision.FCLayerConfig(output_number)
w, b, final = conf_final_fc.apply_layer(mv_net.features)
final = tf.nn.softmax(tf.contrib.layers.flatten(final))
print('RLDRIVER POLICY OUTPUT {}'.format(final.shape))
self.final = final
self.decision_net_args = [w, b]
self.value_net = vision.FCLayerConfig(1)
w, b, value = self.value_net.apply_layer(mv_net.features)
self.value = tf.squeeze(value)
self.value_net_args = [w, b]
print('RLDRIVER VALUE OUTPUT {}'.format(self.value.shape))
sv_sqfeatnum = 16,
mv_featnum = 256,
use_rgb = True, # Obsoleted, always True
master_driver = None,
grads_applier = None
worker_thread_index = -1,
continuous_policy_loss = False):
self.init_state = init_state
self.worker_thread_index = worker_thread_index
if grads_applier is not None:
self.grads_applier = grads_applier
else:
self.grads_applier = tf.AdamOptimizer()
self.action_size = output_number
self.continuous_policy_loss = continuous_policy_loss
self.renderer = pyosr.Renderer()
r = self.renderer
r.pbufferWidth = config.DEFAULT_RES
r.pbufferHeight = config.DEFAULT_RES
if master_driver is None:
r.setup()
r.loadModelFromFile(models[0])
if len(models) > 1 and models[1] is not None:
r.loadRobotFromFile(models[1])
r.state = np.array(init_state, dtype=np.float32)
print('robot loaded')
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
else:
r.setupFrom(master_driver.renderer)
def reanimate():
pyosr.init()
dpy = pyosr.create_display()
glctx = pyosr.create_gl_context(dpy)
r = pyosr.Renderer()
r.avi = True
r.pbufferWidth = r.pbufferHeight = 512
r.setup()
if not hasattr(aniconf, 'env_wt_fn'):
r.loadModelFromFile(aniconf.env_fn)
r.loadRobotFromFile(aniconf.rob_fn)
else:
r.loadModelFromFile(aniconf.env_wt_fn)
r.loadRobotFromFile(aniconf.rob_wt_fn)
if hasattr(aniconf, 'rob_ompl_center'):
r.enforceRobotCenter(aniconf.rob_ompl_center)
r.scaleToUnit()
r.angleModel(0.0, 0.0)
r.default_depth = 0.0
# r.views = np.array([[15.0, 110.0]], dtype=np.float32)
r.views = np.array([[0.0, 0.0]], dtype=np.float32)
print(r.scene_matrix)
print(r.robot_matrix)
# r.set_perturbation(uw_random.random_state(0.00))
# r.set_perturbation(uw_random.random_state(0.25))
# r.set_perturbation(np.array([0,0.0,0,0.5,0.5,0.5,0.5],dtype=np.float32))
# r.set_perturbation(np.array([0,0.0,0,0,0,1,0],dtype=np.float32))
# return
class ReAnimator(object):
reaching_terminal = False
driver = None
im = None
keys = 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
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
state = interpolate(pkey, nkey, tau)
r.state = r.translate_to_unit_state(state)
print("tau {} TO NKEY {}".format(
tau,
pyosr.distance(r.state, r.translate_to_unit_state(nkey))))
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
fig = plt.figure()
keys = np.loadtxt(aniconf.keys_fn)
# keys = np.loadtxt('rrt-secondhalf.path')
if aniconf.keys_w_last:
# if True:
print('before keys[0] {}'.format(keys[0]))
keys[:, [3, 4, 5, 6]] = keys[:, [6, 3, 4, 5]]
print('after keys[0] {}'.format(keys[0]))
# ra = ReAnimator(r, keys, 1.0)
ra = ReAnimator(r, keys, 0.0625)
'''
st0 = np.array([21.782108575648873,11.070742691783639,13.072090341969885,0.99496368307688909,-0.050573680994590003,0.08255004745739393,0.025981951687884433], dtype=np.float64)
st1 = np.array([24.404447383193428,16.614281021136808,17.241012748680941,0.89856334412742611,-0.42392368380753659,0.035352511370216902,0.10780921499298371], dtype=np.float64)
st2 = np.array([25.04292893291256,16.429006629785405,21.742598419634952,-0.080755517119222811,-0.980264716314169,0.167639957003235,0.066756851485538532], dtype=np.float64)
ra = ReAnimator(r, [st1, st2], 0.0125)
'''
'''
st1 = np.array([21.75988005530629,19.55840991214458,18.407298399116954,0.8747148780179097,-0.40598294544114955,0.21522777832862061,0.15404133737658857], dtype=np.float64)
st2 = np.array([16.242401343877191,15.371074546390151,23.775856491398514,0.38753152804205182,-0.26626876971877833,-0.75270143169934201,-0.46082622729571437], dtype=np.float64)
ra = ReAnimator(r, [st1, st2], 0.0125/2.0)
'''
FPS = 60
ani = animation.FuncAnimation(fig, ra.perform, interval=1000 / FPS)
plt.show()
