def trajectory_to_caction(vs, uw, amag):
path = np.array(vs)
for i, (cv, nv) in enumerate(zip(path[:-1], path[1:])):
assert uw.is_valid_state(cv), "{} is not valid state".format(cv)
mag = pyosr.distance(cv, nv)
tr, aa, dquat = pyosr.differential(cv, nv)
applied = pyosr.apply(cv, tr, aa)
if True:
print("mag {}".format(mag))
print("cv {}".format(cv))
print("nv {}".format(nv))
print("tr {}".format(tr))
print("aa {}".format(aa))
print("dquat {}".format(dquat))
print("applied {}".format(applied))
print("applied quat norm {}".format(np.linalg.norm(applied[3:])))
assert pyosr.distance(
applied, nv
) < 1e-6, "pyosr.apply is not the inverse of pyosr.differential {} != {}".format(
applied, nv)
action_step = 0
stepping_tr = tr / mag * amag
stepping_aa = aa / mag * amag
cc = np.copy(cv)
while pyosr.distance(cc, nv) > amag:
yield cc, stepping_tr, stepping_aa
if uw.is_disentangled(cc):
return
cc = pyosr.apply(cc, stepping_tr, stepping_aa)
last_tr, last_aa, dquat = pyosr.differential(cc, nv)
yield cc, last_tr, last_aa
def __iter__(self):
envir = self.envir
sess = self.sess
reaching_terminal = False
args = self.args
pprefix = "[0] "
traj_s = [envir.qstate]
traj_a = []
traj_fv = [self._fv()]
print("Initial state {}".format(envir.qstate))
print("MS list {}".format(self.ms))
rgb, _ = envir.vstate
yield rgb[self.gview] # First view
for ms in self.ms:
print("Switch MS to {}".format(ms))
while True:
print("Current state {}".format(envir.qstate))
sa = []
sd = []
rt = []
rw = []
for a in args.actionset:
nstate, r, reaching_terminal, _ = envir.peek_act(a)
sa.append(nstate)
sd.append(pyosr.distance(nstate, ms))
rw.append(r)
rt.append(reaching_terminal)
print("sa {} \nsd {}".format(sa, sd))
ai = np.argmin(sd)
if sd[ai] > pyosr.distance(envir.qstate,
ms): # Local minimum reached
break
a = args.actionset[ai]
traj_a.append(a)
envir.qstate = sa[ai]
traj_s.append(envir.qstate)
traj_fv.append(self._fv())
rgb, _ = envir.vstate
if rw[ai] < 0:
print("#############################################")
print("!!!!!!!!!! CRITICAL: COLLISION !!!!!!!!!!")
print("!!!!!!!!!! PRESS ENTER TO EXIT !!!!!!!!!!")
input("#############################################")
np.savez(args.sampleout + '.die',
TRAJ_S=traj_s,
TRAJ_A=traj_a,
TRAJ_FV=traj_fv)
return
yield
if rt[ai]:
_press_enter()
np.savez(args.sampleout,
TRAJ_S=traj_s,
TRAJ_A=traj_a,
TRAJ_FV=traj_fv)
return
yield
yield rgb[self.gview] # First view
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
def __iter__(self):
r = self.renderer
while True:
r.render_mvrgbd()
yield np.copy(r.mvrgb.reshape(self.rgb_shape)[0])
if self.target is None:
texts = input("Goal State (W-Last)").split()
self.target = np.array(texts, dtype=np.float64)
self.target[[3, 4, 5, 6]] = self.target[[6, 3, 4, 5]]
self.target = r.translate_to_unit_state(self.target)
DA = uw_random.DISCRETE_ACTION_NUMBER
ns = np.zeros((DA, 7))
d = np.zeros((DA))
for action in range(DA):
nstate, _, ratio = r.transit_state(r.state, action,
self.action_magnitude,
self.verify_magnitude)
ns[action] = nstate
if ratio < 1e-4:
d[action] = 999.9
else:
d[action] = pyosr.distance(nstate, self.target)
print("\tA {} NS {} Ratio {} D {}".format(
action, nstate, ratio, d[action]))
best = np.argmax(d)
bstate = ns[best]
r.state = bstate
def ivi_to_leaving_trajectory(ivi, V, N, D, amag, uw):
# print("N[8050] {}".format(N[8050]))
# ivi = 20146
# ivi = 0
cvi = ivi
vs = []
while True:
# print('CHECKING {} {}'.format(cvi, V[cvi]))
uv = uw.translate_to_unit_state(V[cvi])
if vs and pyosr.distance(uv, vs[-1]) < amag / 10.0:
pass # Do not add extremly close vertices
else:
#if uv[3] < 0.0:
# uv[3:] *= -1.0
vs.append(uv)
vs.append(uv)
if uw.is_disentangled(vs[-1]):
break
cvi = N[cvi]
if cvi < 0:
break
if uw.is_disentangled(vs[-1]):
return vs
else:
return []
def _get_gt_action_from_sample(self, q):
distances = pyosr.multi_distance(q, self.unit_tunnel_v)
ni = np.argmin(distances)
close = self.unit_tunnel_v[ni]
tr, aa, dq = pyosr.differential(q, close)
assert pyosr.distance(close, pyosr.apply(
q, tr, aa)) < 1e-6, "pyosr.differential is not pyosr.apply ^ -1"
return np.concatenate([tr, aa], axis=-1), close
def partition(key0, key1, thresh):
d = pyosr.distance(key0, key1)
print("{}\n {}\n\tdist: {}".format(key0, key1, d))
if d < thresh:
return []
mid = interpolate(key0, key1, 0.5)
print("\t\tmid: {}".format(mid))
firstlist = partition(key0, mid, thresh)
secondlist = partition(mid, key1, thresh)
return firstlist + [mid] + secondlist
def _uvrender_worker(uv_args):
import pyosr
args_dir, uvproj_list, rname = uv_args
ws = util.Workspace(args_dir)
r = util.create_offscreen_renderer(
ws.local_ws(util.TRAINING_DIR, util.PUZZLE_CFG_FILE))
TYPE_TO_FLAG = {
'rob': r.BARY_RENDERING_ROBOT,
'env': r.BARY_RENDERING_SCENE
}
keys = _get_keys(ws)
rflag = TYPE_TO_FLAG[rname]
chart_resolution = np.array([ws.chart_resolution, ws.chart_resolution],
dtype=np.int32)
afb = None
afb_uw = None # Uniform weight
util.log('[uvrender_worker] rendering {} {}'.format(rname, uvproj_list))
for fn in progressbar(uvproj_list):
if DUMMY:
continue
f = matio.load(fn)
i = 0
buffed = 0
try:
for grn, grp in f.items():
IBV = grp['V.{}'.format(rname)][:]
IF = grp['F.{}'.format(rname)][:]
tq = grp['tq']
iq = keys[grp['fromi']]
distance = np.clip(pyosr.distance(tq, iq), 1e-4, None)
w = 1.0 / distance
r.add_barycentric(IF, IBV, rflag, w)
buffed += IF.shape[0]
# util.log("Buffered {}".format(buffed))
if buffed > 32 * 1024:
fb = r.render_barycentric(rflag,
chart_resolution,
svg_fn='')
fb = texture_format.texture_to_file(fb)
uniform_weight = fb.astype(np.float32)
if afb is None:
afb = fb
afb_uw = uniform_weight
else:
afb += fb
afb_uw += uniform_weight
afb_uw[np.nonzero(afb_uw)] = 1.0
r.clear_barycentric(rflag)
buffed = 0
'''
if i > 16:
break
i += 1
'''
except RuntimeError as e:
util.warn("Cannot access file {}".format(fn))
# break
if buffed > 0:
fb = r.render_barycentric(rflag, chart_resolution, svg_fn='')
uniform_weight = texture_format.texture_to_file(
fb.astype(np.float32))
if afb is None:
afb = fb
afb_uw = uniform_weight
else:
afb += fb
afb_uw += uniform_weight
afb_uw[np.nonzero(afb_uw)] = 1.0
ret = {}
ret[rname] = (afb, afb_uw)
return ret
def uvrender(self):
uw = self._uw
tunnel_v = self._get_tunnel_v()
uw.avi = False
geo_type = self._args.geo_type
TYPE_TO_FLAG = {
'rob': uw.BARY_RENDERING_ROBOT,
'env': uw.BARY_RENDERING_SCENE
}
geo_flag = TYPE_TO_FLAG[geo_type]
vert_id = self._args.vert_id
io_dir = self._args.io_dir
iq = uw.translate_to_unit_state(tunnel_v[vert_id])
afb = None
afb_nw = None
tq_gen = TouchQGenerator(in_dir=io_dir, vert_id=vert_id)
obj_gen = UVObjGenerator(in_dir=io_dir,
geo_type=geo_type,
vert_id=vert_id)
i = 0
DEBUG_UVRENDER = False
for tq, is_inf in tq_gen:
# print('tq {} is_inf {}'.format(tq, is_inf))
IBV, IF = next(obj_gen) # Must, otherwise does not pair
if is_inf:
continue
if IBV is None or IF is None:
print('IBV {}'.format(None))
continue
print('{}: IBV {} IF {}'.format(i, IBV.shape, IF.shape))
if DEBUG_UVRENDER:
svg_fn = '{}.svg'.format(i)
# Paint everything ...
if i == 0 and geo_type == 'rob':
V, F = uw.get_robot_geometry(tq, True)
print("V {}\nF {}".format(V.shape, F.shape))
IF, IBV = uw.intersecting_to_robot_surface(tq, True, V, F)
print("IF {}\nIBV {}\n{}".format(IF.shape, IBV.shape,
IBV[:5]))
'''
NPICK=3000
IF = IF[:NPICK]
IBV = IBV[:NPICK*3]
'''
else:
svg_fn = ''
uw.clear_barycentric(geo_flag)
uw.add_barycentric(IF, IBV, geo_flag)
if DEBUG_UVRENDER and i == 2:
print("BaryF {}".format(IF))
print("Bary {}".format(IBV))
fb = uw.render_barycentric(geo_flag,
np.array([ATLAS_RES, ATLAS_RES],
dtype=np.int32),
svg_fn=svg_fn)
#np.clip(fb, 0, 1, out=fb) # Clip to binary
nw = texture_format.texture_to_file(fb.astype(np.float32))
distance = np.clip(pyosr.distance(tq, iq), 1e-4, None)
w = nw * (1.0 / distance)
if afb is None:
afb = w
afb_nw = nw
else:
afb += w
afb_nw += nw
np.clip(afb_nw, 0, 1.0,
out=afb_nw) # afb_nw is supposed to be binary
# Debugging code
if DEBUG_UVRENDER:
print('afb shape {}'.format(afb.shape))
print('distance {}'.format(distance))
rgb = np.zeros(list(afb.shape) + [3])
rgb[..., 1] = w
imsave(_fn_atlastex(io_dir, geo_type, vert_id, i), rgb)
np.savez(atlas_fn(io_dir, geo_type, vert_id, i), w)
print('NW NZ {}'.format(nw[np.nonzero(nw)]))
V1, F1 = uw.get_robot_geometry(tq, True)
pyosr.save_obj_1(V1, F1, '{}.obj'.format(i))
V2, F2 = uw.get_scene_geometry(tq, True)
pyosr.save_obj_1(V2, F2, '{}e.obj'.format(i))
if i >= 16:
return
i += 1
rgb = np.zeros(list(afb.shape) + [3])
rgb[..., 1] = afb
# FIXME: Give savez an explicity array name
imsave(_fn_atlastex(io_dir, geo_type, vert_id, None), rgb)
np.savez(atlas_fn(io_dir, geo_type, vert_id, None), afb)
rgb[..., 1] = afb_nw
imsave(_fn_atlastex(io_dir, geo_type, vert_id, None, nw=True), rgb)
np.savez(atlas_fn(io_dir, geo_type, vert_id, None, nw=True), afb)
def __iter__(self):
envir = self.envir
sess = self.sess
advcore = self.advcore
for _ in self.sample_generator():
q = uw_random.random_state(0.75)
envir.qstate = q
if False:
for i in range(len(self.unit_tunnel_v)):
envir.qstate = self.unit_tunnel_v[i]
yield envir.vstate[0][self.gview]
DEBUG2 = False
if DEBUG2:
assert self.args.samplein, "DEBUG2=True requires --samplein"
fn = '{}/{}.npz'.format(self.args.samplein, self.sample_index)
print(fn)
self.sample_index += 1
d = np.load(fn)
qs = d['QS']
dqs = d['DQS']
closes = d['CLOSES']
for q, dq, close in zip(qs, dqs, closes):
#tr,aa,_ = pyosr.differential(q, close)
#dq = np.concatenate([tr, aa], axis=-1)
app = pyosr.apply(q, dq[:3], dq[3:])
print("q {}\ndq {}\nclose {}\napplied {}".format(
q, dq, close, app))
print("diff {}\n".format(dq))
assert pyosr.distance(close, pyosr.apply(
q, dq[:3], dq[3:])) < 1e-3
# continue
envir.qstate = q
yield envir.vstate[0][self.gview]
input("########## PRESS ENTER FOR APPLIED DQ ##########")
envir.qstate = app
yield envir.vstate[0][self.gview]
input("########## PRESS ENTER FOR CLOSEST Q ##########")
envir.qstate = close
yield envir.vstate[0][self.gview]
input("######## PRESS ENTER FOR THE NEXT TUPLE ########")
continue # Skip the remaining
DEBUG3 = False
if DEBUG3:
for q in self.unit_tunnel_v:
envir.qstate = q
yield envir.vstate[0][self.gview]
continue
DEBUG = False
if DEBUG:
yield envir.vstate[0][self.gview]
distances = pyosr.multi_distance(q, self.unit_tunnel_v)
ni = np.argmin(distances)
close = self.unit_tunnel_v[ni]
envir.qstate = close
yield envir.vstate[0][self.gview]
continue
assert self.args.sampleout, '--sampleout is required for checking CR results'
for i in range(3):
vstate = envir.vstate
yield vstate[0][self.gview]
if envir.r.is_valid_state(envir.qstate):
print("===== Current State is Valid ====")
if i == 1: # Only capture the state after first movement
self.cr_noneed += 1
else:
print("!!!!! Current State is not Valid ====")
print("!DEBUG UNIT: {}".format(envir.qstate))
nustate = envir.r.translate_from_unit_state(envir.qstate)
print("!DEBUG ORIGINAL: {}".format(nustate))
SEARCH_NUMERICAL = False
SEARCH_SA_FORCE = True
if SEARCH_NUMERICAL and i == 1: # Only capture the state after first movement
early_exit = False
current = envir.qstate
self.to_cr_states.append(current)
for k in range(32):
nexts = []
for tr, aa in zip(self.delta_tr, self.delta_aa):
nexts.append(pyosr.apply(current, tr, aa))
nexts = np.array(nexts)
SAs = envir.r.intersection_region_surface_areas(
nexts, True)
print(SAs)
min_index = np.argmin(SAs)
current = nexts[min_index]
envir.qstate = current
vstate = envir.vstate
yield vstate[0][self.gview]
if envir.r.is_valid_state(envir.qstate):
early_exit = True
break
if early_exit:
self.cr_success += 1
self.cr_state_indicators.append(1)
else:
self.cr_fail += 1
self.cr_state_indicators.append(-1)
self.cr_states.append(envir.qstate)
self.sample_index += 1
if SEARCH_SA_FORCE and i == 1: # Force from optimizing surface area
# TODO: Unfinished, just print the force mag and direction
tup = envir.r.force_from_intersecting_surface_area(
envir.qstate)
print('Force Pos:\n{}'.format(tup[0]))
print('Force Direction:\n{}'.format(tup[1]))
print('Force Mag:\n{}'.format(tup[2]))
dic = {
advcore.rgb_tensor: [vstate[0]],
advcore.dep_tensor: [vstate[1]],
}
disp_pred = sess_no_hook(sess,
advcore.finder_pred,
feed_dict=dic)
disp_pred = disp_pred[0][
0] # first batch first view for [B,V,...]
nstate = pyosr.apply(envir.qstate, disp_pred[:3],
disp_pred[3:])
distances = pyosr.multi_distance(q, self.unit_tunnel_v)
ni = np.argmin(distances)
close = self.unit_tunnel_v[ni]
# nstate = envir.r.translate_to_unit_state(nstate_raw)
print("Prediction {}".format(disp_pred))
print("Next (Unit) {}".format(nstate))
print("Error {}".format(pyosr.distance(nstate, close)))
envir.qstate = nstate
# input("########## PRESS ENTER TO CONTINUE ##########")
print("CR No Need {}\nCR SUCCESS {}\nCR FAIL{}".format(
self.cr_noneed, self.cr_success, self.cr_fail))
np.savez(self.args.sampleout,
CRQS=self.cr_states,
TOCRQS=self.to_cr_states,
CRI=self.cr_state_indicators)
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)
