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 train(self, envir, sess, tid=None, tmax=-1):
advcore = self.advcore
'''
Train coarse net
'''
samples = [self._sample_one(envir) for i in range(self.args.batch)]
batch_rgb = [s[0][0] for s in samples]
batch_dep = [s[0][1] for s in samples]
batch_dq = [[s[1]] for s in samples]
ndic = {'rgb': batch_rgb, 'dep': batch_dep, 'dq': batch_dq}
self.dispatch_training_to(sess, ndic, self.coarse_bag,
self.coarse_train_op)
dic = self.ndic_bag_inner_join(ndic, self.coarse_bag)
[pred1] = curiosity.sess_no_hook(sess, [advcore.fine_pred],
feed_dict=dic)
'''
Train fine net
'''
samples2 = [
self._sample_one(envir,
istate=pyosr.apply(samples[i][2], pred1[i, 0, :3],
pred1[i, 0, 3:]))
for i in range(self.args.batch)
]
batch_rgb = [s[0][0] for s in samples2]
batch_dep = [s[0][1] for s in samples2]
batch_dq = [[s[1]] for s in samples2]
ndic = {'rgb': batch_rgb, 'dep': batch_dep, 'dq': batch_dq}
self.dispatch_training_to(sess, ndic, self.fine_bag,
self.fine_train_op)
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 calc_touch(uw, vertex, batch_size):
q0 = uw.translate_to_unit_state(vertex)
N_RET = 5
ret_lists = [[] for i in range(N_RET)]
for i in range(batch_size):
tr = uw_random.random_on_sphere(1.0)
aa = uw_random.random_within_sphere(2 * math.pi)
to = pyosr.apply(q0, tr, aa)
tups = uw.transit_state_to_with_contact(q0, to, STEPPING_FOR_TOUCH)
for i in range(N_RET):
ret_lists[i].append(tups[i])
rets = [np.array(ret_lists[i]) for i in range(N_RET)]
for i in range(N_RET):
print("{} shape {}".format(i, rets[i].shape))
return rets
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 sample_one_touch(uw, q0, stepping):
import pyosr
tr = uw_random.random_on_sphere(1.0)
aa = uw_random.random_within_sphere(2 * math.pi)
to = pyosr.apply(q0, tr, aa)
return uw.transit_state_to_with_contact(q0, to, stepping)