def init(self, boot_spec):
# TODO: do the 1D version
shape = boot_spec.get_observations().shape()
if len(shape) > 2:
msg = 'BGDSagent only work with 2D or 1D signals.'
raise UnsupportedSpec(msg)
min_width = np.min(shape)
if min_width <= 5:
msg = ('BGDSagent thinks this shape is too'
'small to compute gradients: %s' % str(shape))
raise UnsupportedSpec(msg)
self.is2D = len(boot_spec.get_observations().shape()) == 2
self.is1D = len(boot_spec.get_observations().shape()) == 1
ExpSwitcher.init(self, boot_spec)
self.count = 0
self.y_deriv = DerivativeBox()
self.bgds_estimator = BGDSEstimator()
self.model = None
self.y_disag = Expectation()
self.y_disag_s = Expectation()
self.u_stats = []
self.last_y0 = None
self.last_y = None
self.rd = RemoveDoubles(0.5)
def __init__(self, rcond=1e-10, antisym_T=False, antisym_M=False, use_P_scaling=False):
"""
:param rcond: Threshold for computing pseudo-inverse of P.
:param antisym_T: If True, the estimate of T is antisymmetrized.
:param antisym_M: If True, the estimate of M is antisymmetrized.
"""
self.rcond = rcond
self.antisym_M = antisym_M
self.antisym_T = antisym_T
self.use_P_scaling = use_P_scaling
self.info('rcond: %f' % rcond)
self.info('antisym_T: %s' % antisym_T)
self.info('antisym_M: %s' % antisym_M)
self.info('use_P_scaling: %s' % use_P_scaling)
self.T = Expectation()
self.U = Expectation()
self.y_stats = MeanCovariance()
self.u_stats = MeanCovariance()
self.nsamples = 0
self.once = False
class BGDSAgent(ExpSwitcher):
'''
Skip: only consider every $skip observations.
scales: list of floats, represents the scales at which the
sensels are analyzed. 0=raw data, 1= convolved with sigma=1.
'''
@contract(scales='list[>=1](number,>=0)')
def __init__(self, beta, skip=1, scales=[0], fixed_dt=0):
ExpSwitcher.__init__(self, beta)
self.skip = skip
self.scales = scales
self.fixed_dt = fixed_dt
def init(self, boot_spec):
# TODO: do the 1D version
shape = boot_spec.get_observations().shape()
if len(shape) > 2:
msg = 'BGDSagent only work with 2D or 1D signals.'
raise UnsupportedSpec(msg)
min_width = np.min(shape)
if min_width <= 5:
msg = ('BGDSagent thinks this shape is too'
'small to compute gradients: %s' % str(shape))
raise UnsupportedSpec(msg)
self.is2D = len(boot_spec.get_observations().shape()) == 2
self.is1D = len(boot_spec.get_observations().shape()) == 1
ExpSwitcher.init(self, boot_spec)
self.count = 0
self.y_deriv = DerivativeBox()
self.bgds_estimator = BGDSEstimator()
self.model = None
self.y_disag = Expectation()
self.y_disag_s = Expectation()
self.u_stats = []
self.last_y0 = None
self.last_y = None
self.rd = RemoveDoubles(0.5)
def process_observations(self, obs):
dt = float(obs['dt'])
u = obs['commands']
y0 = obs['observations']
episode_start = obs['episode_start']
self.count += 1
if self.count % self.skip != 0:
return
if self.fixed_dt:
# dt is not reliable sometime
# you don't want to give high weight to higher dt samples.
dt = 1 # XXX: add in constants
self.rd.update(y0)
if not self.rd.ready():
return
if self.is2D:
y = create_scales(y0, self.scales)
else:
y = y0
if episode_start:
self.y_deriv.reset()
return
self.y_deriv.update(y, dt)
if not self.y_deriv.ready():
return
y_sync, y_dot_sync = self.y_deriv.get_value()
self.bgds_estimator.update(u=u.astype('float32'),
y=y_sync.astype('float32'),
y_dot=y_dot_sync.astype('float32'),
dt=dt)
self.last_y0 = y0
self.last_y = y
# TODO: implement this separately
if False and self.is2D and self.count > MINIMUM_FOR_PREDICTION:
# TODO: do for 1D
if self.count % 200 == 0 or self.model is None:
self.info('Updating BGDS model.')
self.model = self.bgds_estimator.get_model()
gy = self.bgds_estimator.last_gy
y_dot_est = self.model.estimate_y_dot(y, u, gy=gy)
y_dot_corr = y_dot_est * y_dot_sync
self.y_disag.update(np.maximum(-y_dot_corr, 0))
self.y_disag_s.update(np.sign(y_dot_corr))
u_est = self.model.estimate_u(y, y_dot_sync, gy=gy)
data = {'u': u,
'u_est': u_est,
'timestamp': obs.time,
'id_episode': obs.id_episode
}
self.u_stats.append(data)
# u_est = self.model.estimate_u(y, y_dot_sync, gy=self.bgds_estimator)
# self.u_stats.append()
#
def publish(self, pub):
if self.count < 10:
self.info('Skipping publishing as count=%d' % self.count)
return
self.bgds_estimator.publish(pub.section('model'))
if False and self.is2D: # TODO: implement separately
sec = pub.section('preprocessing')
sec.array_as_image('last_y0', self.last_y0, filter='scale')
sec.array_as_image('last_y', self.last_y, filter='scale')
example = np.zeros(self.last_y.shape)
example.flat[150] = 1
example_smooth = create_scales(example, self.scales)
sec.array_as_image('example_smooth', example_smooth)
if self.count > MINIMUM_FOR_PREDICTION:
sec = pub.section('reliability')
sec.array_as_image('y_disag',
self.y_disag.get_value(), filter='posneg')
sec.array_as_image('y_disag_s',
self.y_disag_s.get_value(), filter='posneg')
if False: # XXX
self.publish_u_stats(pub.section('u_stats'))
def publish_u_stats(self, pub):
T = len(self.u_stats)
print('Obtained %d obs' % T)
K = 2 # FIXME: change this
u_act = np.zeros((T, K))
u_est = np.zeros((T, K))
u_mis = np.zeros((T, K))
u_suc = np.zeros((T, K))
time = np.zeros(T)
num_episode = np.zeros(T, 'int')
id_episode2num = {}
num2id_episode = {}
id_episode2start = {}
# cmd2faults = {}
for t, stats in enumerate(self.u_stats):
u_act[t, :] = stats['u']
u_est[t, :] = stats['u_est']
time[t] = stats['timestamp']
id_ep = stats['id_episode']
if not id_ep in id_episode2num:
id_episode2num[id_ep] = len(id_episode2num)
id_episode2start[id_ep] = time[t]
num2id_episode[id_episode2num[id_ep]] = id_ep
num_episode[t] = id_episode2num[id_ep]
s = ""
for k, v in id_episode2num.items():
s += '%s: %s\n' % (k, v)
pub.text('episodes', s)
with pub.plot('num_episode') as pylab:
pylab.plot(num_episode, '-')
pylab.xlabel('index')
pylab.ylabel('num\_episode')
for id_episode, num in id_episode2num.items():
print id_episode
S = pub.section('Episode:%s' % id_episode)
# times for this episode
et = num_episode == num
# normalize from 0
e_timestamps = time[et]
log_start = e_timestamps[0]
e_timestamps -= log_start
cmd2color = {0: 'g', 1: 'b'}
episode_bounds = (18, 60)
markersize = 2
with S.plot('mis', figsize=(8, 2), mime=MIME_PDF) as pylab:
for k in range(K):
# scale = 7
# u_mis_smooth = scipy.signal.convolve(u_mis[et, k],
# np.ones(scale) / scale,
# mode='same')
pylab.plot(e_timestamps, u_mis[et, k], # u_mis_smooth,
'%s-' % cmd2color[k], label='u[%d]' % k,
markersize=markersize)
x_axis_set(pylab, episode_bounds[0], episode_bounds[1])
with S.plot('success', figsize=(8, 2), mime=MIME_PDF) as pylab:
pylab.plot(e_timestamps, e_timestamps * 0, 'k--')
pylab.plot(e_timestamps, np.ones(len(e_timestamps)), 'k--')
for k in range(K):
pylab.plot(e_timestamps, u_suc[et, k],
'%s-' % cmd2color[k], label='cmd #%d' % k)
y_axis_set(pylab, -0.05, 1.05)
x_axis_set(pylab, episode_bounds[0], episode_bounds[1])
pylab.legend(loc='lower right')
for k in range(K):
with S.plot('commands_%d' % k, figsize=(8, 2),
mime=MIME_PDF) as pylab:
pylab.plot(e_timestamps, u_act[et, k], 'y.',
label='actual', markersize=3)
plot_with_colors(pylab, e_timestamps,
u_est[et, k], u_act[et, k],
markersize=markersize)
y_axis_set(pylab, -2, 2)
x_axis_set(pylab, episode_bounds[0], episode_bounds[1])
def get_predictor(self):
model = self.bgds_estimator.get_model()
return BGDSPredictor(model)
class BDSEEstimator(BDSEEstimatorInterface):
"""
Estimates a BDSE model.
Tensors used: ::
M^s_vi (N) x (N x K)
N^s_i (N) x (K)
T^svi (NxNxK)
U^si (NxK)
"""
@contract(rcond='float,>0')
def __init__(self, rcond=1e-10, antisym_T=False, antisym_M=False, use_P_scaling=False):
"""
:param rcond: Threshold for computing pseudo-inverse of P.
:param antisym_T: If True, the estimate of T is antisymmetrized.
:param antisym_M: If True, the estimate of M is antisymmetrized.
"""
self.rcond = rcond
self.antisym_M = antisym_M
self.antisym_T = antisym_T
self.use_P_scaling = use_P_scaling
self.info('rcond: %f' % rcond)
self.info('antisym_T: %s' % antisym_T)
self.info('antisym_M: %s' % antisym_M)
self.info('use_P_scaling: %s' % use_P_scaling)
self.T = Expectation()
self.U = Expectation()
self.y_stats = MeanCovariance()
self.u_stats = MeanCovariance()
self.nsamples = 0
self.once = False
def merge(self, other):
assert isinstance(other, BDSEEstimator)
self.T.merge(other.T)
self.U.merge(other.U)
self.y_stats.merge(other.y_stats)
self.u_stats.merge(other.u_stats)
self.nsamples += other.nsamples
@contract(u='array[K],K>0,finite',
y='array[N],N>0,finite',
y_dot='array[N],finite', w='>0')
def update(self, y, u, y_dot, w=1.0):
self.once = True
self.nsamples += 1
self.n = y.size
self.k = u.size # XXX: check
self.y_stats.update(y, w)
self.u_stats.update(u, w)
# remove mean
u_n = u - self.u_stats.get_mean()
y_n = y - self.y_stats.get_mean()
# make products
T_k = outer(outer(y_n, y_dot), u_n)
assert T_k.shape == (self.n, self.n, self.k)
U_k = outer(y_dot, u_n)
assert U_k.shape == (self.n, self.k)
# update tensor
self.T.update(T_k, w)
self.U.update(U_k, w)
def get_P_inv_cond(self):
P = self.y_stats.get_covariance()
if False:
P_inv = np.linalg.pinv(P, rcond=self.rcond)
if True:
P2 = P + np.eye(P.shape[0]) * self.rcond
P_inv = np.linalg.inv(P2)
return P_inv
def get_T(self):
T = self.T.get_value()
if self.antisym_T:
self.info('antisymmetrizing T')
T = antisym(T)
return T
def get_model(self):
T = self.get_T()
U = self.U.get_value()
P = self.y_stats.get_covariance()
Q = self.u_stats.get_covariance()
P_inv = self.get_P_inv_cond()
Q_inv = np.linalg.pinv(Q)
if False:
M = get_M_from_P_T_Q(P, T, Q)
else:
if hasattr(self, 'use_P_scaling') and self.use_P_scaling:
M = get_M_from_P_T_Q_alt_scaling(P, T, Q)
else:
warnings.warn('untested')
try:
M = get_M_from_Pinv_T_Q(P_inv, T, Q)
except LinAlgError as e:
msg = 'Could not get_M_from_Pinv_T_Q.\n'
msg += indent(traceback.format_exc(e), '> ')
raise BDSEEstimatorInterface.NotReady(msg)
UQ_inv = np.tensordot(U, Q_inv, axes=(1, 0))
# This works but badly conditioned
Myav = np.tensordot(M, self.y_stats.get_mean(), axes=(1, 0))
N = UQ_inv - Myav
if self.antisym_M:
self.info('antisymmetrizing M')
M = antisym(M)
# # Note: this does not work, don't know why
# if False:
# printm('MYav1', Myav)
# y2 = np.linalg.solve(P, self.y_stats.get_mean())
# Myav2 = np.tensordot(T, y2, axes=(0, 0))
# # Myav = np.tensordot(T, y2, axes=(1, 0))
# printm('MYav2', Myav2)
# if False:
# printm('U', U, 'Q_inv', Q_inv)
# printm('UQ_inv', UQ_inv, 'Myav', Myav, 'N', N)
# printm('u_mean', self.u_stats.get_mean())
# printm('u_std', np.sqrt(Q.diagonal()))
# printm('y_mean', self.y_stats.get_mean())
self.Myav = Myav
self.UQ_inv = UQ_inv
return BDSEmodel(M, N)
def publish(self, pub):
if not self.once:
pub.text('warning', 'not updated yet')
return
pub.text('nsamples', '%s' % self.nsamples)
pub.text('rcond', '%g' % self.rcond)
with pub.subsection('model') as sub:
try:
model = self.get_model()
model.publish(sub)
except BDSEEstimatorInterface.NotReady as e:
pub.text('not-ready', str(e))
with pub.subsection('tensors') as sub:
T = self.get_T()
U = self.U.get_value()
P = self.y_stats.get_covariance()
Q = self.u_stats.get_covariance()
P_inv = np.linalg.pinv(P)
P_inv_cond = self.get_P_inv_cond()
Q_inv = np.linalg.pinv(Q)
#
# TP_inv2 = obtain_TP_inv_from_TP_2(T, P)
# M2 = np.tensordot(TP_inv2, Q_inv, axes=(2, 0))
pub_tensor3_slice2(sub, 'T', T)
pub_tensor2_comp1(sub, 'U', U)
pub_tensor2_cov(sub, 'P', P, rcond=self.rcond)
pub_tensor2_cov(sub, 'P_inv', P_inv)
pub_tensor2_cov(sub, 'P_inv_cond', P_inv_cond)
pub_tensor2_cov(sub, 'Q', Q)
pub_tensor2_cov(sub, 'Q_inv', Q_inv)
# Might not have been computed
# pub_tensor2_comp1(sub, 'Myav', self.Myav)
# pub_tensor2_comp1(sub, 'UQ_inv', self.UQ_inv)
with pub.subsection('y_stats') as sub:
self.y_stats.publish(sub)
with pub.subsection('u_stats') as sub:
self.u_stats.publish(sub)
with pub.subsection('alternative', robust=True) as sub:
sub.text('info', 'This is estimating without conditioning P')
T = self.get_T()
P = self.y_stats.get_covariance()
Q = self.u_stats.get_covariance()
M1 = get_M_from_P_T_Q(P, T, Q)
pub_tensor3_slice2(sub, 'get_M_from_P_T_Q', M1)
M2 = get_M_from_P_T_Q_alt(P, T, Q)
pub_tensor3_slice2(sub, 'get_M_from_P_T_Q_alt', M2)
M3 = get_M_from_P_T_Q_alt_scaling(P, T, Q)
pub_tensor3_slice2(sub, 'get_M_from_P_T_Q_alt2', M3)
