def test_quaternion_imitate():
T = np.linspace(0, 2, 101)
n_features = 20
alpha = 25.0
widths = np.empty(n_features)
centers = np.empty(n_features)
dmp.initialize_rbf(widths, centers, T[-1], T[0], 0.8, alpha / 3.0)
R = np.empty((T.shape[0], 4))
for i in range(T.shape[0]):
angle = T[i] * np.pi
R[i] = np.array([
np.cos(angle / 2.0),
np.sqrt(0.5) * np.sin(angle / 2.0),
np.sqrt(0.5) * np.sin(angle / 2.0), 0.0
])
weights = np.empty((n_features, 3))
dmp.quaternion_imitate(T, R, weights, widths, centers, 1e-10, alpha,
alpha / 4.0, alpha / 3.0, False)
last_t = T[0]
last_r = R[0].copy()
last_rd = np.zeros(3)
last_rdd = np.zeros(3)
r = R[0].copy()
rd = np.zeros(3)
rdd = np.zeros(3)
g = R[-1].copy()
gd = np.zeros(3)
gdd = np.zeros(3)
r0 = R[0].copy()
r0d = np.zeros(3)
r0dd = np.zeros(3)
R_replay = []
for t in T:
dmp.quaternion_dmp_step(last_t, t, last_r, last_rd, last_rdd, r, rd,
rdd, g, gd, gdd, r0, r0d, r0dd, T[-1], T[0],
weights, widths, centers, alpha, alpha / 4.0,
alpha / 3.0, 0.001)
last_t = t
last_r[:] = r
last_rd[:] = rd
last_rdd[:] = rdd
R_replay.append(r.copy())
R_replay = np.asarray(R_replay)
distances = np.array(
[np.linalg.norm(r - r_replay) for r, r_replay in zip(R, R_replay)])
assert_less(distances.max(), 0.043)
assert_less(distances.min(), 1e-10)
assert_less(sorted(distances)[len(distances) // 2], 0.02)
assert_less(np.mean(distances), 0.02)
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 test_quaternion_step():
last_r = np.array([0.0, 1.0, 0.0, 0.0])
last_rd = np.array([0.0, 0.0, 0.0])
last_rdd = np.array([0.0, 0.0, 0.0])
r = np.array([0.0, 1.0, 0.0, 0.0])
rd = np.array([0.0, 0.0, 0.0])
rdd = np.array([0.0, 0.0, 0.0])
g = np.array([0.0, 0.0, 7.07106781e-01, 7.07106781e-01])
gd = np.array([0.0, 0.0, 0.0])
gdd = np.array([0.0, 0.0, 0.0])
r0 = np.array([0.0, 1.0, 0.0, 0.0])
r0d = np.array([0.0, 0.0, 0.0])
r0dd = np.array([0.0, 0.0, 0.0])
n_features = 10
weights = np.zeros((n_features, 3))
execution_time = 1.0
alpha = 25.0
widths = np.empty(n_features)
centers = np.empty(n_features)
dmp.initialize_rbf(widths, centers, execution_time, 0.0, 0.8, alpha / 3.0)
T = np.linspace(0.0, 1.0 * execution_time, 1001)
last_t = 0.0
R_replay = []
for t in T:
dmp.quaternion_dmp_step(last_t, t, last_r, last_rd, last_rdd, r, rd,
rdd, g, gd, gdd, r0, r0d, r0dd, execution_time,
0.0, weights, widths, centers, alpha,
alpha / 4.0, alpha / 3.0, 0.001)
last_t = t
last_r[:] = r
last_rd[:] = rd
last_rdd[:] = rdd
R_replay.append(r.copy())
R_replay = np.asarray(R_replay)
assert_array_almost_equal(r, g, decimal=4)
assert_array_almost_equal(rd, gd, decimal=3)
assert_array_almost_equal(rdd, gdd, decimal=2)
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