def make_data(self):
if self.data is None:
return
XU = self.data
if self.config.predict_difference:
dtheta = math_utils.angular_diff_batch(XU[1:, 0], XU[:-1, 0])
dtheta_dt = XU[1:, 1] - XU[:-1, 1]
Y = torch.cat((dtheta.view(-1, 1), dtheta_dt.view(-1, 1)),
dim=1) # x' - x residual
else:
Y = XU[1:, :2]
XU = XU[:-1] # make same size as Y
self.N = XU.shape[0]
self.config.load_data_info(XU[:, :2], XU[:, 2:], Y)
if self.preprocessor:
self.preprocessor.tsf.fit(XU, Y)
self.preprocessor.update_data_config(self.config)
self._original_train = XU, Y, None
self._original_val = self._original_train
XU, Y, _ = self.preprocessor.tsf.transform(XU, Y)
self._train = XU, Y, None
self._val = self._train
def __call__(self, X, U=None, terminal=False):
original_angle = self.oxy - self.rxy
original_angle = torch.atan2(original_angle[1], original_angle[0])
Xxy = X[:, :2]
d = self.oxy - Xxy
angles = torch.atan2(d[:, 1], d[:, 0])
angular_diffs = math_utils.angular_diff_batch(original_angle, angles) * torch.norm(d,dim=1)
return angular_diffs * (1 if self.clockwise else -1)
def update_model():
# recreate prior if necessary (for non-network based priors; since they contain data directly)
# pm = prior.LSQPrior.from_data(ds)
# pm = prior.GMMPrior.from_data(ds)
# ctrl.update_prior(pm)
# # update model based on database change (for global linear controllers)
# ctrl.update_model(ds)
# retrain network (for nn dynamics based controllers)
mw.train()
mw.learn_model(TRAIN_EPOCH, batch_N=500)
mw.eval()
# evaluate network against true dynamics
yt = true_dynamics(statev, actionv)
yp = dynamics(statev, actionv)
dtheta = math_utils.angular_diff_batch(yp[:, 0], yt[:, 0])
dtheta_dt = yp[:, 1] - yt[:, 1]
E = torch.cat((dtheta.view(-1, 1), dtheta_dt.view(-1, 1)),
dim=1).norm(dim=1)
logger.info("Error with true dynamics theta %f theta_dt %f norm %f",
dtheta.abs().mean(),
dtheta_dt.abs().mean(), E.mean())
logger.debug("Start next collection sequence")
def _process_file_raw_data(self, d):
x = d['X']
if x.shape[1] > PushWithForceDirectlyEnv.nx:
x = x[:, :PushWithForceDirectlyEnv.nx]
# from testing, it's better if these guys are delayed 1 time step (to prevent breaking causality)
# ignore force in z
r = d['reaction'][:, :2]
if self.config.predict_difference:
dpos = x[1:, :2] - x[:-1, :2]
dyaw = math_utils.angular_diff_batch(x[1:, 2], x[:-1, 2])
dalong = x[1:, 3] - x[:-1, 3]
dr = r[1:] - r[:-1]
y = np.concatenate(
(dpos, dyaw.reshape(-1, 1), dalong.reshape(-1, 1), dr), axis=1)
else:
raise RuntimeError(
"Too hard to predict discontinuous normalized angles; use predict difference"
)
x = np.concatenate((x, r), axis=1)
xu, y, cc = self._apply_masks(d, x, y)
return xu, y, cc
def compare_to_goal(state, goal):
dtheta = math_utils.angular_diff_batch(state[:, 0], goal[:, 0])
dtheta_dt = state[:, 1] - goal[:, 1]
return dtheta.reshape(-1, 1), dtheta_dt.reshape(-1, 1)
def state_difference(state, other_state):
dyaw = math_utils.angular_diff_batch(state[:, 2], other_state[:, 2])
dpos = state[:, :2] - other_state[:, :2]
dalong = state[:, 3] - other_state[:, 3]
return dpos, dyaw.reshape(-1, 1), dalong.reshape(-1, 1)
def state_difference(state, other_state):
"""Get state - other_state in state space"""
dyaw = math_utils.angular_diff_batch(state[:, 4], other_state[:, 4])
dpos = state[:, :4] - other_state[:, :4]
return dpos, dyaw.reshape(-1, 1)
def state_difference(state, other_state):
dyaw = math_utils.angular_diff_batch(state[:, 2], other_state[:, 2])
dpos = state[:, :2] - other_state[:, :2]
dreaction = state[:, 3:5] - other_state[:, 3:5]
return dpos, dyaw.reshape(-1, 1), dreaction