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)