def step(self):
"""Compute next step."""
if self.n_task_dims == 0:
return
dmp_idx = np.where(self.steps <= self.split_steps)[0][0]
dmp.dmp_step(self.last_t, self.t, self.last_y, self.last_yd,
self.last_ydd, self.y, self.yd, self.ydd,
self.subgoals[dmp_idx + 1],
self.subgoal_velocities[dmp_idx + 1],
np.zeros(self.n_task_dims), self.subgoals[dmp_idx],
self.subgoal_velocities[dmp_idx],
np.zeros(self.n_task_dims),
np.sum(self.execution_times[:dmp_idx + 1]),
np.sum(self.execution_times[:dmp_idx]),
self.weights[dmp_idx], self.widths[dmp_idx],
self.centers[dmp_idx], self.alpha_y, self.beta_y,
self.alpha_z[dmp_idx], 0.001)
if self.t == self.last_t:
self.last_t = -1.0
else:
self.last_t = self.t
self.t += self.dt
self.steps += 1
def execute(T, Y, weights, widths, centers, alpha):
last_t = T[0]
last_y = Y[0].copy()
last_yd = np.zeros(2)
last_ydd = np.zeros(2)
y = Y[0].copy()
yd = np.zeros(2)
ydd = np.zeros(2)
g = Y[-1].copy()
gd = np.zeros(2)
gdd = np.zeros(2)
y0 = Y[0].copy()
y0d = np.zeros(2)
y0dd = np.zeros(2)
for t in np.linspace(T[0], T[-1], T.shape[0]):
dmp.dmp_step(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,
False, alpha, alpha / 4.0, alpha / 3.0, 0.001)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
def trajectory(self):
"""Generate trajectory represented by the sequence of DMPs in open loop.
The function can be used for debugging purposes.
Returns
-------
X : array, shape (n_steps, n_task_dims)
Positions
Xd : array, shape (n_steps, n_task_dims)
Velocities
Xdd : array, shape (n_steps, n_task_dims)
Accelerations
"""
last_t = 0.0
last_y = np.copy(self.subgoals[0])
last_yd = np.copy(self.subgoal_velocities[0])
last_ydd = np.zeros(self.n_task_dims)
y = np.empty(self.n_task_dims)
yd = np.empty(self.n_task_dims)
ydd = np.empty(self.n_task_dims)
n_steps = int(sum(self.execution_times) / self.dt) + 1
Y = np.empty([n_steps, self.n_task_dims])
Yd = np.empty([n_steps, self.n_task_dims])
Ydd = np.empty([n_steps, self.n_task_dims])
steps = 0
for t in np.arange(0, sum(self.execution_times) + self.dt, self.dt):
dmp_idx = np.where(steps <= self.split_steps)[0][0]
dmp.dmp_step(last_t, t, last_y, last_yd, last_ydd, y, yd, ydd,
self.subgoals[dmp_idx + 1],
self.subgoal_velocities[dmp_idx + 1],
np.zeros(self.n_task_dims), self.subgoals[dmp_idx],
self.subgoal_velocities[dmp_idx],
np.zeros(self.n_task_dims),
np.sum(self.execution_times[:dmp_idx + 1]),
np.sum(self.execution_times[:dmp_idx]),
self.weights[dmp_idx], self.widths[dmp_idx],
self.centers[dmp_idx], self.alpha_y, self.beta_y,
self.alpha_z[dmp_idx], 0.001)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
Y[steps, :] = y.copy()
Yd[steps, :] = yd.copy()
Ydd[steps, :] = ydd.copy()
steps += 1
return Y, Yd, Ydd
def trajectory(self):
"""Generate trajectory represented by the DMP in open loop.
The function can be used for debugging purposes.
Returns
-------
X : array, shape (n_steps, 7)
Positions and rotations (order: x, y, z, w, rx, ry, rz)
"""
last_t = 0.0
last_y = np.copy(self.x0)
last_yd = np.copy(self.x0d)
last_ydd = np.copy(self.x0dd)
y = np.empty(3)
yd = np.empty(3)
ydd = np.empty(3)
last_r = np.copy(self.q0)
last_rd = np.copy(self.q0d)
last_rdd = np.copy(self.q0dd)
r = np.empty(4)
rd = np.empty(3)
rdd = np.empty(3)
Y = []
R = []
for t in np.arange(0, self.execution_time + self.dt, self.dt):
dmp.dmp_step(last_t, t, last_y, last_yd, last_ydd, y, yd, ydd,
self.g, self.gd, self.gdd, self.x0, self.x0d,
self.x0dd, self.execution_time, 0.0,
self.position_weights, self.widths, self.centers,
self.alpha_y, self.beta_y, self.alpha_z, 0.001)
dmp.quaternion_dmp_step(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_z, 0.001)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
last_r[:] = r
last_rd[:] = rd
last_rdd[:] = rdd
Y.append(y.copy())
R.append(r.copy())
return np.hstack((Y, R))
def trajectory(self):
"""Generate trajectory represented by the DMP in open loop.
The function can be used for debugging purposes.
Returns
-------
X : array, shape (n_steps, n_task_dims)
Positions
Xd : array, shape (n_steps, n_task_dims)
Velocities
Xdd : array, shape (n_steps, n_task_dims)
Accelerations
"""
last_t = 0.0
last_y = np.copy(self.x0)
last_yd = np.copy(self.x0d)
last_ydd = np.copy(self.x0dd)
y = np.empty(self.n_task_dims)
yd = np.empty(self.n_task_dims)
ydd = np.empty(self.n_task_dims)
Y = []
Yd = []
Ydd = []
for t in np.linspace(0, self.execution_time,
round((self.execution_time + self.dt) / self.dt)):
dmp.dmp_step(
last_t, t,
last_y, last_yd, last_ydd,
y, yd, ydd,
self.g, self.gd, self.gdd,
self.x0, self.x0d, self.x0dd,
self.execution_time, 0.0,
self.weights,
self.widths,
self.centers,
self.alpha_y, self.beta_y, self.alpha_z,
0.001
)
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.asarray(Y), np.asarray(Yd), np.asarray(Ydd)
def test_imitate():
T = np.linspace(0, 2, 101)
n_features = 9
widths = np.empty(n_features)
centers = np.empty(n_features)
dmp.initialize_rbf(widths, centers, T[-1], T[0], 0.8, 25.0 / 3.0)
Y = np.hstack((T[:, np.newaxis], np.cos(np.pi * T)[:, np.newaxis]))
weights = np.empty((n_features, 2))
alpha = 25.0
dmp.imitate(T, Y, weights, widths, centers, 1e-10, alpha, alpha / 4.0,
alpha / 3.0, False)
last_t = T[0]
last_y = Y[0].copy()
last_yd = np.zeros(2)
last_ydd = np.zeros(2)
y = Y[0].copy()
yd = np.zeros(2)
ydd = np.zeros(2)
g = Y[-1].copy()
gd = np.zeros(2)
gdd = np.zeros(2)
y0 = Y[0].copy()
y0d = np.zeros(2)
y0dd = np.zeros(2)
Y_replay = []
for t in np.linspace(T[0], T[-1], T.shape[0]):
dmp.dmp_step(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.001)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
Y_replay.append(y.copy())
Y_replay = np.asarray(Y_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)
def step(self):
"""Compute desired position, velocity and acceleration."""
if self.n_task_dims == 0:
return
dmp.dmp_step(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.x0, self.x0d, self.x0dd,
self.execution_time, 0.0, self.weights, self.widths,
self.centers, self.alpha_y, self.beta_y, self.alpha_z,
0.001)
if self.t == self.last_t:
self.last_t = -1.0
else:
self.last_t = self.t
self.t += self.dt
def step(self):
"""Compute desired position, velocity and acceleration."""
dmp.dmp_step(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.x0, self.x0d, self.x0dd,
self.execution_time, 0.0, self.position_weights,
self.widths, self.centers, self.alpha_y, self.beta_y,
self.alpha_z, 0.001)
dmp.quaternion_dmp_step(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_z, 0.001)
if self.t == self.last_t:
self.last_t = -1.0
else:
self.last_t = self.t
self.t += self.dt
def test_step():
last_y = np.array([0.0])
last_yd = np.array([0.0])
last_ydd = np.array([0.0])
y = np.empty([1])
yd = np.empty([1])
ydd = np.empty([1])
g = np.array([1.0])
gd = np.array([0.0])
gdd = np.array([0.0])
n_weights = 10
weights = np.zeros((n_weights, 1))
execution_time = 1.0
alpha = 25.0
widths = np.empty(n_weights)
centers = np.empty(n_weights)
dmp.initialize_rbf(widths, centers, execution_time, 0.0, 0.8, alpha / 3.0)
last_t = 0.0
# Execute DMP longer than expected duration
for t in np.linspace(0.0, 1.5 * execution_time, 151):
dmp.dmp_step(last_t, t, last_y, last_yd, last_ydd, y, yd, ydd,
g, gd, gdd, np.array([0.0]), np.array([0.0]),
np.array([0.0]), execution_time, 0.0, weights, widths,
centers, alpha, alpha / 4.0, alpha / 3.0, 0.001)
last_t = t
last_y[:] = y
last_yd[:] = yd
last_ydd[:] = ydd
assert_array_almost_equal(y, g, decimal=6)
assert_array_almost_equal(yd, gd, decimal=5)
assert_array_almost_equal(ydd, gdd, decimal=4)
