def rollout(self, tau = 1.0, v0 = None, **kwargs):
'''
Generate a system trial, no feedback is incorporated.
tau scalar, time rescaling constant
v0 scalar, initial velocity of the system
'''
# Reset the state of the DMP
if v0 is None:
v0 = 0.0 * self.x_0
self.reset_state(v0 = v0)
x_track = np.array([self.x_0])
dx_track = np.array([v0])
t_track = np.array([0])
state = np.zeros(2 * self.n_dmps)
state[range(0, 2*self.n_dmps, 2)] = copy.deepcopy(v0)
state[range(1, 2*self.n_dmps + 1, 2)] = copy.deepcopy(self.x_0)
if self.rescale == 'rotodilatation':
M = roto_dilatation(self.learned_position, self.x_goal - self.x_0)
elif self.rescale == 'diagonal':
M = np.diag((self.x_goal - self.x_0) / self.learned_position)
else:
M = np.eye(self.n_dmps)
psi = self.gen_psi(self.cs.s)
f0 = (np.dot(self.w, psi[:, 0])) / (np.sum(psi[:, 0])) * self.cs.s
f0 = np.nan_to_num(np.dot(M, f0))
ddx_track = np.array([-self.D * v0 + self.K*f0])
err = np.linalg.norm(state[range(1, 2*self.n_dmps + 1, 2)] - self.x_goal)
P = phi1(self.cs.dt * self.linear_part / tau)
while err > self.tol:
psi = self.gen_psi(self.cs.s)
f = (np.dot(self.w, psi[:, 0])) / (np.sum(psi[:, 0])) * self.cs.s
f = np.nan_to_num(np.dot(M, f))
beta = np.zeros(2 * self.n_dmps)
beta[range(0, 2*self.n_dmps, 2)] = \
self.K * (self.x_goal * (1.0 - self.cs.s) +
self.x_0 * self.cs.s + f) / tau
vect_field = np.dot(self.linear_part / tau, state) + beta
state += self.cs.dt * np.dot(P, vect_field)
x_track = np.append(x_track,
np.array([state[range(1, 2*self.n_dmps + 1, 2)]]), axis=0)
dx_track = np.append(dx_track,
np.array([state[range(0, 2*self.n_dmps, 2)]]), axis=0)
t_track = np.append(t_track, t_track[-1] + self.cs.dt)
err = np.linalg.norm(state[range(1, 2*self.n_dmps + 1, 2)] - self.x_goal)
self.cs.step(tau=tau)
ddx_track = np.append(ddx_track,
np.array([self.K * (self.x_goal - x_track[-1]) -
self.D * dx_track[-1] -
self.K * (self.x_goal - self.x_0) * self.cs.s +
self.K * f]), axis=0)
return x_track, dx_track, ddx_track, t_track
def step(self,
tau=1.0,
error=0.0,
external_force=None,
adapt=False,
tols=None,
**kwargs):
'''
Run the DMP system for a single timestep.
tau float: time rescaling constant
error float: optional system feedback
external_force 1D array: external force to add to the system
adapt bool: says if using adaptive step
tols float list: [rel_tol, abs_tol]
'''
## Initialize
if tols is None:
tols = [1e-03, 1e-06]
## Setup
# Scaling matrix
if self.rescale == 'rotodilatation':
M = roto_dilatation(self.learned_position, self.x_goal - self.x_0)
elif self.rescale == 'diagonal':
M = np.diag((self.x_goal - self.x_0) / self.learned_position)
else:
M = np.eye(self.n_dmps)
# Coupling term in canonical system
error_coupling = 1.0 / (1.0 + error)
alpha_tilde = -self.cs.alpha_s / tau / error_coupling
# State definition
state = np.zeros(2 * self.n_dmps)
state[0::2] = self.dx
state[1::2] = self.x
# Linear part of the dynamical system
A_m = self.linear_part / tau
# s-dep part of the dynamical system
def beta_s(s, x, v):
psi = self.gen_psi(s)
f = (np.dot(self.w, psi[:, 0])) / (np.sum(psi[:, 0])) * self.cs.s
f = np.nan_to_num(np.dot(M, f))
out = np.zeros(2 * self.n_dmps)
out[0::2] = self.K * (self.x_goal * (1.0 - s) + self.x_0 * s + f)
if external_force is not None:
out[0::2] += external_force(x, v)
return out / tau
## Initialization of the adaptive step
flag_tol = False
while not flag_tol:
# Bogacki–Shampine method
# Defining the canonical system in the time of the scheme
s1 = copy.deepcopy(self.cs.s)
s2 = s1 * np.exp(-alpha_tilde * self.cs.dt * 1.0 / 2.0)
s3 = s1 * np.exp(-alpha_tilde * self.cs.dt * 3.0 / 4.0)
s4 = s1 * np.exp(-alpha_tilde * self.cs.dt)
xi1 = np.dot(A_m, state) + beta_s(s1, state[1::2], state[0::2])
xi2 = np.dot(A_m, state + self.cs.dt * xi1 * 1.0/2.0) + \
beta_s(s2, state[1::2] + self.cs.dt * xi1[1::2] * 1.0/2.0,
state[0::2] + self.cs.dt * xi1[0::2] * 1.0/2.0)
xi3 = np.dot(A_m, state + self.cs.dt * xi2 * 3.0/4.0) + \
beta_s(s3, state[1::2] + self.cs.dt * xi2[1::2] * 3.0/4.0,
state[0::2] + self.cs.dt * xi2[0::2] * 3.0/4.0)
xi4 = np.dot(A_m, state + self.cs.dt * (2.0 * xi1 + 3.0 * xi2 + 4.0 * xi3) / 9.0) + \
beta_s(s4, state[1::2] + self.cs.dt * (2.0 * xi1[1::2] + 3.0 * xi2[1::2] + 4.0 * xi3[1::2]) / 9.0,
state[0::2] + self.cs.dt * (2.0 * xi1[0::2] + 3.0 * xi2[0::2] + 4.0 * xi3[0::2]) / 9.0)
y_ord2 = state + self.cs.dt * (2.0 * xi1 + 3.0 * xi2 +
4.0 * xi3) / 9.0
y_ord3 = state + self.cs.dt * (7.0 * xi1 + 6.0 * xi2 + 8.0 * xi3 +
3.0 * xi4) / 24.0
if (np.linalg.norm(y_ord2 - y_ord3) <
tols[0] * np.linalg.norm(state) + tols[1]) or (not adapt):
flag_tol = True
state = copy.deepcopy(y_ord3)
else:
self.cs.dt /= 1.1
self.cs.step()
self.x = copy.deepcopy(state[1::2])
self.dx = copy.deepcopy(state[0::2])
psi = self.gen_psi(self.cs.s)
f = (np.dot(self.w, psi[:, 0])) / (np.sum(psi[:, 0])) * self.cs.s
f = np.nan_to_num(np.dot(M, f))
self.ddx = (self.K * (self.x_goal - self.x) - self.D * self.dx \
- self.K * (self.x_goal - self.x_0) * self.cs.s + self.K * f) / tau
if external_force is not None:
self.ddx += external_force(self.x, self.dx) / tau
return self.x, self.dx, self.ddx
def paths_regression(self, traj_set, t_set=None):
'''
Takes in a set (list) of desired trajectories (with possibly the
execution times) and generate the weight which realize the best
approximation.
each element of traj_set should be shaped num_timesteps x n_dim
trajectories
'''
## Step 1: Generate the set of the forcing terms
f_set = np.zeros([len(traj_set), self.n_dmps, self.cs.timesteps])
g_new = np.ones(self.n_dmps)
for it in range(len(traj_set)):
if t_set is None:
t_des_tmp = None
else:
t_des_tmp = t_set[it]
t_des_tmp -= t_des_tmp[0]
t_des_tmp /= t_des_tmp[-1]
t_des_tmp *= self.cs.run_time
# Alignment of the trajectory so that
# x_0 = [0; 0; ...; 0] and g = [1; 1; ...; 1].
x_des_tmp = copy.deepcopy(traj_set[it])
x_des_tmp -= x_des_tmp[0] # translation to x_0 = 0
g_old = x_des_tmp[-1] # original x_goal position
R = roto_dilatation(g_old, g_new) # rotodilatation
# Rescaled and rotated trajectory
x_des_tmp = np.dot(x_des_tmp, np.transpose(R))
# Learning of the forcing term for the particular trajectory
f_tmp = self.imitate_path(x_des=x_des_tmp,
t_des=t_des_tmp,
g_w=False,
add_force=None)
f_set[
it, :, :] = f_tmp.copy() # add the new forcing term to the set
## Step 2: Learning of the weights using linear regression
self.w = np.zeros([self.n_dmps, self.n_bfs + 1])
s_track = self.cs.rollout()
psi_set = self.gen_psi(s_track)
psi_sum = np.sum(psi_set, 0)
psi_sum_2 = psi_sum * psi_sum
s_track_2 = s_track * s_track
A = np.zeros([self.n_bfs + 1, self.n_bfs + 1])
for k in range(self.n_bfs + 1):
A[k, k] = scipy.integrate.simps(
psi_set[k, :] * psi_set[k, :] * s_track_2 / psi_sum_2, s_track)
for h in range(k + 1, self.n_bfs + 1):
A[h, k] = scipy.integrate.simps(
psi_set[k, :] * psi_set[h, :] * s_track_2 / psi_sum_2,
s_track)
A[k, h] = A[h, k].copy()
A *= len(traj_set)
LU = scipy.linalg.lu_factor(A)
# The weights are learned dimension by dimension
for d in range(self.n_dmps):
f_d_set = f_set[:, d, :].copy()
# Set up the minimization problem
b = np.zeros([self.n_bfs + 1])
for k in range(self.n_bfs + 1):
b[k] = scipy.integrate.simps(
np.sum(f_d_set * psi_set[k, :] * s_track / psi_sum, 0),
s_track)
# Solve the minimization problem
self.w[d, :] = scipy.linalg.lu_solve(LU, b)
self.learned_position = np.ones(self.n_dmps)
rho = 1. - np.random.rand() / 5.
x0 = rho * np.array([np.cos(theta), np.sin(theta)])
tf = 6. + 2. * (np.random.rand() - 0.5)
m = int (500 + np.floor(500 * np.random.rand()))
t_set.append(np.linspace(0, tf, m))
# Execute the trajectory
X = RK4(x0, m, tf)
traj_set.append(X.copy())
# Plot
plt.figure(1)
plt.plot(X[:, 0], X[:, 1], '-b', lw = 0.5)
plt.axis('equal')
# Plot after translation and roto dilatation
Z = X - X[0]
old_pos = Z[-1]
R = roto_dilatation(old_pos, np.array([1,1]))
Z = np.dot(Z, R.transpose())
plt.figure(2)
plt.plot(Z[:, 0], Z[:, 1], '-b', lw = 0.5)
plt.axis('equal')
MP = dmp(n_dmps = 2, n_bfs = 50, K = 1000, alpha_s = 4.,rescale = 'rotodilatation', T = 2.)
MP.paths_regression(traj_set, t_set)
x_track, _, _, _ = MP.rollout()
plt.figure(2)
plt.plot(x_track[:, 0], x_track[:, 1], '-r', lw = 2)
plt.xlabel(r'$x_1$')
plt.ylabel(r'$x_2$')
plt.plot(0, 0, '.k', markersize = 10)
plt.plot(1, 1, '*k', markersize = 10)
plt.title('Scaled reference frame')