def __init__(self,
x,
ell_prior1=None,
ell_prior2=None,
a1=None,
b1=None,
a2=None,
b2=None,
ngl1=100,
ngl2=100):
"""
GPCSD1D Spatial covariance (Squared exponential).
:param x: spatial locations of observed LFP in 2D (n_spatial_lfp, 2)
:param ell_prior1: GPCSDPrior instance or None, prior for lengthscale in first spatial dim
:param ell_prior2: GPCSDPrior instance or None, prior for lengthscale in second spatial dim
:param a1: lower boundary for integration in first spatial dim
:param b1: upper boundary for integration in first spatial dim
:param a2: lower boundary for integration in second spatial dim
:param b2: upper boundary for integration in second spatial dim
:param ngl1: number of points to use in integration in first spatial dim
:param ngl2: number of points to use in integration in second spatial dim
"""
GPCSD2DSpatialCov.__init__(self, x, a1, b1, a2, b2, ngl1, ngl2)
x1, x2 = reduce_grid(x)
if ell_prior1 is None:
ell_prior1 = GPCSDInvGammaPrior()
lb = 2.0 * np.min(np.diff(np.sort(x1).squeeze()))
ub = 2.0 * (np.max(np.sort(x1).squeeze()) -
np.min(np.sort(x1).squeeze()))
ell_prior1.set_params(lb, ub)
if ell_prior2 is None:
ell_prior2 = GPCSDInvGammaPrior()
lb = 2.0 * np.min(np.diff(np.sort(x2).squeeze()))
ub = (np.max(np.sort(x2).squeeze()) -
np.min(np.sort(x2).squeeze()))
ell_prior2.set_params(lb, ub)
# setup ell1 param
ell1 = ell_prior1.sample()
ell_min1 = np.min(np.diff(np.sort(x1).squeeze()))
ell_max1 = 5.0 * np.max(np.sort(x1).squeeze()) - np.min(
np.sort(x1).squeeze())
# setup ell2 param
ell2 = ell_prior2.sample()
ell_min2 = np.min(np.diff(np.sort(x2).squeeze()))
ell_max2 = np.max(np.sort(x2).squeeze()) - np.min(
np.sort(x2).squeeze())
self.params = {
'ell1': {
'value': ell1,
'prior': ell_prior1,
'min': ell_min1,
'max': ell_max1
},
'ell2': {
'value': ell2,
'prior': ell_prior2,
'min': ell_min2,
'max': ell_max2
}
}
def rle(stateseq):
"""
Compute the run length encoding of a discrete state sequence.
E.g. the state sequence [0, 0, 1, 1, 1, 2, 3, 3]
would be encoded as ([0, 1, 2, 3], [2, 3, 1, 2])
[Copied from pyhsmm.util.general.rle]
Parameters
----------
stateseq : array_like
discrete state sequence
Returns
-------
ids : array_like
integer identities of the states
durations : array_like (int)
length of time in corresponding state
"""
pos, = np.where(np.diff(stateseq) != 0)
pos = np.concatenate(([0],pos+1,[len(stateseq)]))
return stateseq[pos[:-1]], np.diff(pos)
def __init__(self, x, ell_prior=None, a=None, b=None, ngl=100):
"""
GPCSD1D Spatial covariance (Squared exponential).
:param x: spatial locations of observed LFP
:param ell_prior: GPCSDPrior instance or None
:param a: lower boundary for integration
:param b: upper boundary for integration
:param ngl: number of points to use in integration
"""
GPCSD1DSpatialCov.__init__(self, x, a, b, ngl)
if ell_prior is None:
ell_prior = GPCSDInvGammaPrior()
lb = 1.2 * np.min(np.diff(self.x.squeeze()))
ub = 0.8 * (np.max(self.x.squeeze()) - np.min(self.x.squeeze()))
ell_prior.set_params(lb, ub)
ell = ell_prior.sample()
ell_min = 0.5 * np.min(np.diff(self.x.squeeze()))
ell_max = np.max(self.x.squeeze()) - np.min(self.x.squeeze())
self.params = {
'ell': {
'value': ell,
'prior': ell_prior,
'min': ell_min,
'max': ell_max
}
}
def __init__(self, t, ell_prior=None, sigma2_prior=None):
GPCSDTemporalCov.__init__(self, t)
if ell_prior is None:
ell_prior = GPCSDInvGammaPrior()
lb = 1.2 * np.min(np.diff(self.t.flatten()))
ub = 0.8 * (np.max(self.t.flatten()) - np.min(self.t.flatten()))
ell_prior.set_params(lb, ub)
if sigma2_prior is None:
sigma2_prior = GPCSDHalfNormalPrior(1.0)
ell_min = 0.5 * np.min(np.diff(self.t.flatten()))
ell_max = (np.max(self.t.flatten()) - np.min(self.t.flatten()))
ell = ell_prior.sample()
sigma2 = sigma2_prior.sample()
self.params = {
'ell': {
'value': ell,
'prior': ell_prior,
'min': ell_min,
'max': ell_max
},
'sigma2': {
'value': sigma2,
'prior': sigma2_prior,
'min': 0,
'max': np.inf
}
}
def cost(self, x):
u = self.forward(x)
diff = (self.obs - u)
negative_log_likelihood = 0.5 * np.dot(
np.dot(diff, self.Gamma_noise_inv), diff)
controls = self.unflatten(x)
penalty_controls = 0. * np.sum(
np.mean(ewm(
1.,
np.power(
np.abs(np.diff(controls[0], axis=0)) -
np.diff(controls[0], axis=0), 1.)),
axis=0))
penalty_controls += np.sum(
np.mean(ewm(1., np.power(np.abs(np.diff(controls[0], axis=0)),
1.)),
axis=0))
# # # penalization on HFR, beta, delta, kappa, sigma, eta_I, eta_Q, mu, gamma_I, gamma_A, gamma_H, gamma_Q from mean
# result = 0.
# for j in range(0, len(parameters)):
# result = result + 0.5/500 * np.sum(np.power(np.divide(np.abs(controls[j+1] - parameters[j]), parameters[j]/2), 1))
#
# penalty_controls += result
print("cost, penalty = ", negative_log_likelihood,
10. * penalty_controls)
return negative_log_likelihood + 10. * penalty_controls
def compute_tv_error(W):
# sq l2 norm for tv error
Wmx,Wmy,Wsx,Wsy,s02,K,kappa,T,XX,XXp,Eta,Xi,h = parse_W(W)
topo_var_list = [arr.reshape(topo_shape+(-1,)) for arr in \
[XX,XXp,Eta,Xi]]
sqdiffy = [np.sum(np.abs(np.diff(top,axis=0))**2) for top in topo_var_list]
sqdiffx = [np.sum(np.abs(np.diff(top,axis=1))**2) for top in topo_var_list]
cost = np.sum(sqdiffy+sqdiffx)
return cost
def get_other_params(q_init, x_fin, y_fin, z_fin):
other_params = {}
n = 50
other_params['n'] = 50
# weight's of cost function
other_params['rho_vel'] = 100.0
other_params['rho_acc'] = 1000.0
other_params['rho_b'] = 1000.0
other_params['rho_jerk'] = 10000.0
other_params['rho_orient'] = 1000.0
other_params['rho_pos'] = 1000.0
# joint limits of franka manipulator
q_min = np.array([-165.0, -100.0, -165.0, -165.0, -165.0, -1.0, -165.0
]) * np.pi / 180
q_max = np.array([165.0, 101.0, 165.0, 1.0, 165.0, 214.0, 165.0
]) * np.pi / 180
other_params['q_min_traj'] = np.hstack((q_min[0]*np.ones(n), q_min[1]*np.ones(n), q_min[2]*np.ones(n),\
q_min[3]*np.ones(n), q_min[4]*np.ones(n), q_min[5]*np.ones(n), \
q_min[6]*np.ones(n)))
other_params['q_max_traj'] = np.hstack((q_max[0]*np.ones(n), q_max[1]*np.ones(n), q_max[2]*np.ones(n), \
q_max[3]*np.ones(n), q_max[4]*np.ones(n), q_max[5]*np.ones(n), \
q_max[6]*np.ones(n)))
# first, second and third order difference matrices
A = np.identity(n)
other_params['A_vel'] = np.diff(A, axis=0)
other_params['A_acc'] = np.diff(other_params['A_vel'], axis=0)
other_params['A_jerk'] = np.diff(other_params['A_acc'], axis=0)
# desired axis-angle for end effector
roll_des = -87.2 * np.pi / 180
pitch_des = -41.0 * np.pi / 180
other_params['roll_des'] = roll_des
other_params['pitch_des'] = pitch_des
# mid point index
other_params['mid_index'] = 25
other_params['q_1_init'] = q_init[0]
other_params['q_2_init'] = q_init[1]
other_params['q_3_init'] = q_init[2]
other_params['q_4_init'] = q_init[3]
other_params['q_5_init'] = q_init[4]
other_params['q_6_init'] = q_init[5]
other_params['q_7_init'] = q_init[6]
other_params['x_fin'] = x_fin
other_params['y_fin'] = y_fin
other_params['z_fin'] = z_fin
return other_params
def compute_tv_error(W1,W2):
# sq l2 norm for tv error
#Wmx,Wmy,Wsx,Wsy,s02,Kin,Kout,kappa,Tin,Tout,XX,XXp,Eta,Xi,h1,h2 = parse_W(W)
W0x,W0y,W1x,W1y,W2x,W2y,W3x,W3y,s02,Kin0,Kin1,Kxout0,Kyout0,Kxout1,Kyout1,kappa,Tin0,Tin1,Txout0,Tyout0,Txout1,Tyout1,h1,h2,bl,amp = parse_W1(W1)
XX,XXp,Eta,Xi = parse_W2(W2)
topo_var_list = [arr[topo_stims].reshape(topo_shape+(-1,)) for arr in \
[XX,XXp,Eta,Xi]]
sqdiffy = [np.sum(np.abs(np.diff(top,axis=0))**2) for top in topo_var_list]
sqdiffx = [np.sum(np.abs(np.diff(top,axis=1))**2) for top in topo_var_list]
cost = np.sum(sqdiffy+sqdiffx)
return cost
def testCategoricalHMMWithKnownStates():
T = 50
K = 3
obsDim = 2
D = 3
mp = CategoricalHMM()
initialDist = Dirichlet.generate(D=K)
transDist = TransitionDirichletPrior.generate(D_in=K, D_out=K)
emissionDist = TransitionDirichletPrior.generate(D_in=K, D_out=obsDim)
ys = [Categorical.generate(D=obsDim, size=T) for _ in range(D)]
start = time.time()
mp.updateParams(initialDist, transDist, emissionDist, ys)
end = time.time()
print('Preprocess: ', end - start)
kS = int(np.random.random() * T / 10) + 2
knownStates = np.random.choice(T, kS)
knownStates = np.vstack(
(knownStates, np.random.choice(K, knownStates.shape[0]))).reshape(
(2, -1)).T
# Sort and remove duplicates
knownStates = np.array(sorted(knownStates, key=lambda x: x[0]))
knownStates = knownStates[1:][~(np.diff(knownStates[:, 0]) == 0)]
# print( knownStates )
start = time.time()
alphas = mp.forwardFilter(knownLatentStates=knownStates)
betas = mp.backwardFilter(knownLatentStates=knownStates)
end = time.time()
print('Both filters: ', end - start)
marginal = np.logaddexp.reduce(alphas[-1])
for a, b in zip(alphas, betas):
# print( a + b )
# comp = np.logaddexp.reduce( a + b )
comp = mp.log_marginalFromAlphaBeta(a, b)
assert np.isclose(comp, marginal), comp - marginal
for t in range(T - 1):
joint = mp.childParentJoint(t, alphas, betas)
parentProb = np.logaddexp.reduce(joint, axis=1)
childProb = np.logaddexp.reduce(joint, axis=0)
trueParent = alphas[t] + betas[t]
trueChild = alphas[t + 1] + betas[t + 1]
assert np.allclose(parentProb, trueParent)
assert np.allclose(childProb, trueChild)
print(
'Passed the categorical forward backward marginal test with known states!\n\n'
)
def compute_tv_error(W):
# sq l2 norm for tv error
W0x, W0y, W1x, W1y, W2x, W2y, W3x, W3y, s02, k0, k1, k2, k3, kappa, T0, T1, T2, T3, XX, XXp, Eta, Xi, h = parse_W(
W)
topo_var_list = [arr.reshape(topo_shape+(-1,)) for arr in \
[XX,XXp,Eta,Xi]]
sqdiffy = [
np.sum(np.abs(np.diff(top, axis=0))**2)
for top in topo_var_list
]
sqdiffx = [
np.sum(np.abs(np.diff(top, axis=1))**2)
for top in topo_var_list
]
cost = np.sum(sqdiffy + sqdiffx)
return cost
def cost(self, controls, states, system_eval_step):
"""
Compute the penalty.
Arguments:
controls
states
system_eval_step
Returns:
cost
"""
if self.max_control_norms is None:
normalized_controls = controls / self.max_control_norms
else:
normalized_controls = controls
# Penalize the square of the absolute value of the difference
# in value of the control parameters from one step to the next.
diffs = anp.diff(normalized_controls, axis=0, n=self.order)
cost = anp.sum(anp.real(diffs * anp.conjugate(diffs)))
# You can prove that the square of the complex modulus of the difference
# between two complex values is l.t.e. 2 if the complex modulus
# of the two complex values is l.t.e. 1 respectively using the
# triangle inequality. This fact generalizes for higher order differences.
# Therefore, a factor of 2 should be used to normalize the diffs.
cost_normalized = cost / self.cost_normalization_constant
return cost_normalized * self.cost_multiplier
def _cumulative_hazard(self, params, T, Xs):
n = T.shape[0]
T = T.reshape((n, 1))
M = np.minimum(np.tile(self.breakpoints, (n, 1)), T)
M = np.hstack([M[:, tuple([0])], np.diff(M, axis=1)])
lambdas_ = np.array([safe_exp(-np.dot(Xs[param], params[param])) for param in self._fitted_parameter_names])
return (M * lambdas_.T).sum(1)
def _cumulative_hazard(self, params, times):
n = times.shape[0]
times = times.reshape((n, 1))
bp = self.breakpoints
M = np.minimum(np.tile(bp, (n, 1)), times)
M = np.hstack([M[:, tuple([0])], np.diff(M, axis=1)])
return np.dot(M, 1 / params)
def neg_mean_D(self, x, c, n, *params):
mask = c == 0
x_obs = x[mask]
n_obs = n[mask]
gamma = 0
# Assumes already ordered
F = self.ff(x_obs - gamma, *params)
D0 = F[0]
Dn = 1 - F[-1]
D = np.diff(F)
D = np.concatenate([np.array([D0]), D.T, np.array([Dn])]).T
Dr = self.sf(x[c == 1] - gamma, *params)
Dl = self.ff(x[c == -1] - gamma, *params)
if (n_obs > 1).any():
n_ties = (n_obs - 1).sum()
Df = self.df(x_obs - gamma, *params)
# Df = Df[Df != 0]
LL = np.concatenate([Dl, Df, Dr])
ll_n = np.concatenate([n[c == -1], (n_obs - 1), n[c == 1]])
else:
Df = []
n_ties = n_obs.sum()
LL = np.concatenate([Dl, Dr])
ll_n = np.concatenate([n[c == -1], n[c == 1]])
M = np.log(D)
M = -np.sum(M) / (M.shape[0])
LL = -(np.log(LL) * ll_n).sum() / (n.sum() - n_obs.sum() + n_ties)
return M + LL
def integrate(self, t, *args, **kwargs):
"""
This integral is a simple linear interpolant,
which I would like to do as a spline.
However, I need to do it manually, since
it needs to be autograd differentiable, which splines are not.
The method here is not especially efficent.
"""
tau = self.get_param('tau', **kwargs)
kappa = self.get_param('kappa', **kwargs)
mu = self.get_param('mu', 0.0, **kwargs)
f_kappa = self.f_kappa(kappa=kappa, mu=mu)
t = np.reshape(t, (-1, 1))
delta = np.diff(tau)
each = np.maximum(
0, (t - tau[:-1].reshape(1, -1))
)
each = np.minimum(
each,
delta.reshape(1, -1)
)
return np.sum(
each * np.reshape(f_kappa, (1, -1)),
1
) + (mu * t.ravel())
def predict_cumulative_hazard(self, df, times=None):
"""
Return the cumulative hazard rate of subjects in X at time points.
Parameters
----------
X: numpy array or DataFrame
a (n,d) covariate numpy array or DataFrame. If a DataFrame, columns
can be in any order. If a numpy array, columns must be in the
same order as the training data.
times: iterable, optional
an iterable of increasing times to predict the cumulative hazard at. Default
is the set of all durations (observed and unobserved). Uses a linear interpolation if
points in time are not in the index.
Returns
-------
cumulative_hazard_ : DataFrame
the cumulative hazard of individuals over the timeline
"""
times = np.asarray(
coalesce(times, self.timeline, np.unique(self.durations)))
n = times.shape[0]
times = times.reshape((n, 1))
lambdas_ = self._prep_inputs_for_prediction_and_return_parameters(df)
bp = self.breakpoints
M = np.minimum(np.tile(bp, (n, 1)), times)
M = np.hstack([M[:, tuple([0])], np.diff(M, axis=1)])
return pd.DataFrame(np.dot(M, (1 / lambdas_)),
columns=_get_index(df),
index=times[:, 0])
def _get_weights(tt):
tt = np.asarray(tt)
W = np.zeros((tt.size, tt.size))
h = np.diff(tt)
for i in range(len(tt)):
W[i, :i] += .5 * h[:i]
W[i, 1:i+1] += .5 * h[:i]
return W
def afir_func(coords3d):
diffs = anp.diff(coords3d[inds], axis=1).reshape(-1, 3)
rs = anp.linalg.norm(diffs, axis=1)
omegas = (cov_rad_sums / rs)**p
f = alpha * rho * (omegas * rs).sum() / omegas.sum()
return f
def _get_weights(tt):
tt = np.asarray(tt)
W = np.zeros((tt.size, tt.size))
h = np.diff(tt)
for i in range(len(tt)):
W[i, :i] += .5 * h[:i]
W[i, 1:i + 1] += .5 * h[:i]
return W
def _round_vals(x):
not_different = True
i = 1
while not_different:
x_ticks = np.array(round_sig(x, i))
not_different = (np.diff(x_ticks) == 0).any()
i += 1
return x_ticks
def testGaussianHMM():
T = 100
K = 20
obsDim = 40
D = 4
mp = GaussianHMM()
initialDist = Dirichlet.generate(D=K)
transDist = TransitionDirichletPrior.generate(D_in=K, D_out=K)
mus, sigmas = list(
zip(*[NormalInverseWishart.generate(D=obsDim) for _ in range(K)]))
ys = np.random.random((D, T, obsDim))
start = time.time()
mp.updateParams(initialDist, transDist, mus, sigmas, ys)
end = time.time()
print('Preprocess: ', end - start)
kS = int(np.random.random() * T / 10) + 2
knownStates = np.random.choice(T, kS)
knownStates = np.vstack(
(knownStates, np.random.choice(K, knownStates.shape[0]))).reshape(
(2, -1)).T
# Sort and remove duplicates
knownStates = np.array(sorted(knownStates, key=lambda x: x[0]))
knownStates = knownStates[1:][~(np.diff(knownStates[:, 0]) == 0)]
start = time.time()
alphas = mp.forwardFilter(knownLatentStates=knownStates)
betas = mp.backwardFilter(knownLatentStates=knownStates)
end = time.time()
print('Both filters: ', end - start)
marginal = np.logaddexp.reduce(alphas[-1])
for a, b in zip(alphas, betas):
# comp = np.logaddexp.reduce( a + b )
comp = mp.log_marginalFromAlphaBeta(a, b)
assert np.isclose(comp, marginal), comp - marginal
for t in range(T - 1):
joint = mp.childParentJoint(t, alphas, betas)
parentProb = np.logaddexp.reduce(joint, axis=1)
childProb = np.logaddexp.reduce(joint, axis=0)
trueParent = alphas[t] + betas[t]
trueChild = alphas[t + 1] + betas[t + 1]
assert np.allclose(parentProb, trueParent)
assert np.allclose(childProb, trueChild)
print('Passed the gaussian forward backward marginal test!\n\n')
def _cumulative_hazard(self, params, T, X):
n = T.shape[0]
T = T.reshape((n, 1))
M = np.minimum(np.tile(self.breakpoints, (n, 1)), T)
M = np.hstack([M[:, tuple([0])], np.diff(M, axis=1)])
lambdas_ = np.array(
[np.exp(-np.dot(X, params[self._LOOKUP_SLICE[param]])) for param in self._fitted_parameter_names]
)
return M * lambdas_.T
def _cumulative_hazard(self, params, times):
warnings.simplefilter(action="ignore", category=FutureWarning)
n = times.shape[0]
times = times.reshape((n, 1))
bp = self.breakpoints
M = np.minimum(np.tile(bp, (n, 1)), times)
M = np.hstack([M[:, tuple([0])], np.diff(M, axis=1)])
return np.dot(M, 1 / params)
def testHMMBasic():
with np.errstate(under='ignore',
divide='raise',
over='raise',
invalid='raise'):
T = 40
D_latent = 3
D_obs = 2
meas = 4
size = 5
initialDist = Dirichlet.generate(D=D_latent)
transDist = TransitionDirichletPrior.generate(D_in=D_latent,
D_out=D_latent)
emissionDist = TransitionDirichletPrior.generate(D_in=D_latent,
D_out=D_obs)
state = HMMState(initialDist=initialDist,
transDist=transDist,
emissionDist=emissionDist)
_, ys = HMMState.generate(measurements=meas,
T=T,
D_latent=D_latent,
D_obs=D_obs,
size=size)
kS = int(np.random.random() * T / 10) + 2
knownStates = np.random.choice(T, kS)
knownStates = np.vstack(
(knownStates, np.random.choice(D_latent,
knownStates.shape[0]))).reshape(
(2, -1)).T
# Sort and remove duplicates
knownStates = np.array(sorted(knownStates, key=lambda x: x[0]))
knownStates = knownStates[1:][~(np.diff(knownStates[:, 0]) == 0)]
xNoCond, ysNoCond = state.isample(T=T, measurements=meas, size=size)
xForward, yForward = state.isample(ys=ys,
knownLatentStates=knownStates)
xBackward, yBackward = state.isample(ys=ys, forwardFilter=False)
state.ilog_likelihood((xNoCond, ysNoCond))
state.ilog_likelihood((xForward, yForward))
state.ilog_likelihood((xBackward, yBackward), forwardFilter=False)
state.ilog_likelihood((xNoCond, ysNoCond), conditionOnY=True)
state.ilog_likelihood((xForward, yForward),
knownLatentStates=knownStates,
conditionOnY=True)
state.ilog_likelihood((xBackward, yBackward),
forwardFilter=False,
conditionOnY=True)
print('Done with basic HMM state test')
def solution(self, x):
x = np.append(0, x)
dx = np.diff(x)
u = np.zeros_like(x)
for i in range(1, len(x)):
u[i] = u[i - 1] + self.beta * u[i - 1] * (1 - u[i - 1] ** 2) / (1 + u[i - 1] ** 2) * self.dt[i - 1] + dx[i - 1]
return u
def _cumulative_hazard(self, params, T, X):
n = T.shape[0]
T = T.reshape((n, 1))
M = np.minimum(np.tile(self.breakpoints, (n, 1)), T)
M = np.hstack([M[:, tuple([0])], np.diff(M, axis=1)])
lambdas_ = np.array([
np.exp(-np.dot(X, params[self._LOOKUP_SLICE["lambda_%d_" % i]]))
for i in range(self.n_breakpoints)
])
return M * lambdas_.T
def project_simplex_bounded(r, lb, ub):
assert lb.sum() <= 1 and ub.sum() >= 1 and np.all(lb <= ub), 'not feasible'
lambdas = np.append(lb - r, ub - r)
idx = np.argsort(lambdas)
lambdas = lambdas[idx]
active = np.cumsum((idx < r.size) * 2 - 1)[:-1]
diffs = np.diff(lambdas, n=1)
totals = lb.sum() + np.cumsum(active * diffs)
i = np.searchsorted(totals, 1.0)
lam = (1 - totals[i]) / active[i] + lambdas[i + 1]
return np.clip(r + lam, lb, ub)
def get_other_params():
other_params = {}
n = 50
other_params['n'] = 50
# weight's of cost function
other_params['rho_vel'] = 100.0
other_params['rho_acc'] = 1000.0
other_params['rho_b'] = 1000.0
other_params['rho_jerk'] = 10000.0
other_params['rho_orient'] = 10.0
# joint limits of franka manipulator
q_min = np.array([-165.0, -100.0, -165.0, -165.0, -165.0, -1.0, -165.0
]) * np.pi / 180
q_max = np.array([165.0, 101.0, 165.0, 1.0, 165.0, 214.0, 165.0
]) * np.pi / 180
other_params['q_min_traj'] = np.hstack((q_min[0]*np.ones(n), q_min[1]*np.ones(n), q_min[2]*np.ones(n),\
q_min[3]*np.ones(n), q_min[4]*np.ones(n), q_min[5]*np.ones(n), \
q_min[6]*np.ones(n)))
other_params['q_max_traj'] = np.hstack((q_max[0]*np.ones(n), q_max[1]*np.ones(n), q_max[2]*np.ones(n), \
q_max[3]*np.ones(n), q_max[4]*np.ones(n), q_max[5]*np.ones(n), \
q_max[6]*np.ones(n)))
# first, second and third order difference matrices
A = np.identity(n)
other_params['A_vel'] = np.diff(A, axis=0)
other_params['A_acc'] = np.diff(other_params['A_vel'], axis=0)
other_params['A_jerk'] = np.diff(other_params['A_acc'], axis=0)
# desired axis-angle for end effector
roll_des = -87.2 * np.pi / 180
pitch_des = -41.0 * np.pi / 180
other_params['roll_des'] = roll_des
other_params['pitch_des'] = pitch_des
return other_params
def set_actions_anchor_points(self, actions):
""" Set action anchor points
Args:
actions (np.array): actions taken by logging policy
"""
self.action_anchor_points = self.bucketizer.get_anchor_points(
actions)[:-1]
bandwidth = np.diff(self.action_anchor_points[:-1]).min() / 2
self.action_bandwidth = bandwidth * self.hyperparams['action_bandwidth']
self.kernel_action = get_kernel_by_name('gaussian')(
self.action_bandwidth)
def plot_trajectory(z, x, ax=None, ls="-"):
zcps = np.concatenate(([0], np.where(np.diff(z))[0] + 1, [z.size]))
if ax is None:
fig = plt.figure(figsize=(4, 4))
ax = fig.gca()
for start, stop in zip(zcps[:-1], zcps[1:]):
ax.plot(x[start:stop + 1, 0],
x[start:stop + 1, 1],
lw=1, ls=ls,
color=colors[z[start] % len(colors)],
alpha=1.0)
return ax
def __make_Phi__(self) -> np.array:
"""
Phi is a matrix for the transformation from Fourier series
coefficients to time series
"""
w = self.hydro.omega.values
dw = np.mean(np.diff(w))
nf = self.hydro.omega.size
T = np.linspace(0, 2*np.pi/dw, 2*nf+1, endpoint=False)
MM = np.reshape(w,(-1,1)) @ np.reshape(T,(1,-1))
Phi = np.ones((2*nf+1, 2*nf+1))
Phi[1::2,::] = np.cos(MM)
Phi[2::2,::] = np.sin(MM)
return Phi
def rle(stateseq):
pos, = np.where(np.diff(stateseq) != 0)
pos = np.concatenate(([0],pos+1,[len(stateseq)]))
return stateseq[pos[:-1]], np.diff(pos)
