def gen_traj(self):
"""
Gnerate the trajectory represented by the DMP in the Open Loop
:return:
"""
last_t = 0.0
last_y = np.copy(self.y0)
last_yd = np.copy(self.y0d)
last_ydd = np.copy(self.y0dd)
y = np.zeros(self.n_task_dims)
yd = np.zeros(self.n_task_dims)
ydd = np.zeros(self.n_task_dims)
Y = []
Yd = []
Ydd = []
for t in np.arange(0, self.execution_time + self.dt, self.dt):
dmp.dmpPropagate(last_t, t, last_y, last_yd, last_ydd, y, yd, ydd,
self.g, self.gd, self.gdd, self.y0, self.y0d,
self.y0dd, self.execution_time, 0.0, self.weights,
self.widths, self.centers, self.alpha_y,
self.beta_y, self.alpha_x, self.dt)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
Y.append(y.copy())
Yd.append(yd.copy())
Ydd.append(ydd.copy())
return [np.array(Y), np.array(Yd), np.array(Ydd)]
def step(self):
"""
Compute desired position, velocity and acceleration of the transformation system
:return:
"""
if self.n_task_dims != 6:
raise ValueError("Task dimensions are not 6!")
dmp.dmpPropagate(self.last_t, self.t,
self.last_y, self.last_yd, self.last_ydd,
self.y, self.yd, self.ydd,
self.g, self.gd, self.gdd,
self.y0, self.y0d, self.y0dd,
self.execution_time, 0.0,
self.position_weights, self.widths, self.centers,
self.alpha_y, self.beta_y, self.alpha_x,
self.dt)
dmp.dmpPropagateQuaternion(self.last_t, self.t,
self.last_r, self.last_rd, self.last_rdd,
self.r, self.rd, self.rdd,
self.qg, self.qgd, self.qgdd,
self.q0, self.q0d, self.q0dd,
self.execution_time, 0.0,
self.orientation_weights, self.widths, self.centers,
self.alpha_y, self.beta_y, self.alpha_x,
self.dt)
self.last_t = self.t
self.t += self.dt
def test_learn_from_demo():
T = np.linspace(0, 2, 201)
n_features = 20
widths = np.zeros(n_features)
centers = np.zeros(n_features)
#dmp.initializeRBF(widths, centers, T[-1], T[0], 0.8, 25 / 3.0)
Y = np.hstack((T[:, np.newaxis], np.hstack((np.sqrt(np.pi * T[:, np.newaxis]), np.sin(np.pi * T[:, np.newaxis])))))
Y = np.hstack((Y, np.sin(np.pi * T[:, np.newaxis]) * np.sqrt(np.pi * T[:, np.newaxis])))
weights = np.zeros((n_features, 1))
alpha = 25.0
#dmp.LearnfromDemo(T, Y, weights, widths, centers, 1e-10, alpha, alpha / 4.0, alpha / 3.0, False)
last_t = T[0]
last_y = np.zeros(1)
last_yd = np.zeros(1)
last_ydd = np.zeros(1)
y0 = np.zeros(1)
y0d = np.zeros(1)
y0dd = np.zeros(1)
y = np.zeros(1)
yd = np.zeros(1)
ydd = np.zeros(1)
g = np.zeros(1)
gd = np.ones(1)
gdd = np.zeros(1)
Y_replay = []
Yd_replay = []
for t in np.linspace(T[0], T[-1], T.shape[0]):
dmp.dmpPropagate(last_t, t,
last_y, last_yd, last_ydd,
y, yd, ydd,
g, gd, gdd,
y0, y0d, y0dd,
T[-1], T[0],
weights,
widths,
centers,
alpha, alpha / 4.0, alpha / 3.0,
0.01)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
Y_replay.append(y.copy())
Yd_replay.append(yd.copy())
Y_replay = np.array(Y_replay)
Yd_replay = np.array(Yd_replay)
distances = np.array([np.linalg.norm(y - y_replay)
for y, y_replay in zip(Y, Y_replay)])
# assert_less(distances.max(), 0.032)
# assert_less(distances.min(), 1e-10)
# assert_less(sorted(distances)[len(distances) // 2], 0.02)
# assert_less(np.mean(distances), 0.02)
# Four axes, returned as a 2-d array
f, axarr = plt.subplots(4, 2)
axarr[0, 0].plot(T, Y_replay[:, 0])
axarr[0, 1].plot(T, Yd_replay[:, 0])
axarr[1, 0].plot(T, Y_replay[:, 1])
axarr[1, 1].plot(T, Yd_replay[:, 1])
axarr[2, 0].plot(T, Y_replay[:, 2])
axarr[2, 1].plot(T, Yd_replay[:, 2])
axarr[3, 0].plot(T, Y_replay[:, 3])
axarr[3, 1].plot(T, Yd_replay[:, 3])
plt.show()
assert(None != None )
def gen_traj(self):
"""
Gnerate the trajectory represented by the DMP in the Open Loop
:return:
"""
last_t = 0.0
last_y = np.copy(self.y0)
last_yd = np.copy(self.y0d)
last_ydd = np.copy(self.y0dd)
last_r = np.copy(self.q0)
last_rd = np.copy(self.q0d)
last_rdd = np.copy(self.q0dd)
y = np.zeros(3)
yd = np.zeros(3)
ydd = np.zeros(3)
r = np.zeros(4)
rd = np.zeros(3)
rdd = np.zeros(3)
Y = []
Yd = []
Ydd = []
R = []
Rd = []
Rdd = []
for t in np.arange(0, self.execution_time + self.dt, self.dt):
dmp.dmpPropagate(last_t, t,
last_y, last_yd, last_ydd,
y, yd, ydd,
self.g, self.gd, self.gdd,
self.y0, self.y0d, self.y0dd,
self.execution_time, 0.0,
self.position_weights, self.widths, self.centers,
self.alpha_y, self.beta_y, self.alpha_x,
self.dt)
dmp.dmpPropagateQuaternion(last_t, t,
last_r, last_rd, last_rdd,
r, rd, rdd,
self.qg, self.qgd, self.qgdd,
self.q0, self.q0d, self.q0dd,
self.execution_time, 0.0,
self.orientation_weights, self.widths, self.centers,
self.alpha_y, self.beta_y, self.alpha_x,
self.dt)
last_t = t
last_r[:] = r
last_rd[:] = rd
last_rdd[:] = rdd
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
Y.append(y.copy())
Yd.append(yd.copy())
Ydd.append(ydd.copy())
R.append(r.copy())
Rd.append(rd.copy())
Rdd.append(rdd.copy())
return np.array(Y), np.array(Yd), np.array(Ydd), \
np.array(R), np.array(Rd), np.array(Rdd)