def test_quaternion_step_invalid_times():
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)
assert_raises_regexp(ValueError,
"Goal must be chronologically after start",
dmp.quaternion_dmp_step, 0.0, 0.1, last_r, last_rd,
last_rdd, r, rd, rdd, g, gd, gdd, r0, r0d, r0dd, 0.0,
0.0, weights, widths, centers, alpha, alpha / 4.0,
alpha / 3.0, 0.001)
def test_step_invalid_times():
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)
assert_raises_regexp(ValueError,
"Goal must be chronologically after start",
dmp.dmp_step, 0.0, 0.1, last_y, last_yd, last_ydd, y,
yd, ydd, g, gd, gdd, np.array([0.0]), np.array([0.0]),
np.array([0.0]), 0.0, 0.0, weights, widths, centers,
alpha, alpha / 4.0, alpha / 3.0, 0.001)
def imitate(T, Y, n_features, alpha):
widths = np.empty(n_features)
centers = np.empty(n_features)
weights = np.empty(n_features * 2)
dmp.initialize_rbf(widths, centers, 1.0, 0.8, 25.0 / 3.0)
dmp.imitate(T, Y.ravel(), weights, widths, centers, 1e-10, alpha,
alpha / 4.0, alpha / 3.0, False)
return widths, centers, weights
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 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 test_imitate_ill_conditioning():
n_features = 101
widths = np.empty(n_features)
centers = np.empty(n_features)
dmp.initialize_rbf(widths, centers, 1.0, 0.0, 0.8, 25.0 / 3.0)
T = np.linspace(0, 1, 101)
Y = np.hstack((T[:, np.newaxis], np.cos(2 * np.pi * T)[:, np.newaxis]))
weights = np.empty((n_features, 2))
alpha = 25.0
assert_raises_regexp(ValueError, "must be >= 0", dmp.imitate, T, Y,
weights, widths, centers, -1.0, alpha, alpha / 4.0,
alpha / 3.0, False)
assert_raises_regexp(ValueError, "instable", dmp.imitate, T, Y, weights,
widths, centers, 0.0, alpha, alpha / 4.0, alpha / 3.0,
False)
def init(self, n_inputs, n_outputs):
"""Initialize the behavior.
Parameters
----------
n_inputs : int
number of inputs
n_outputs : int
number of outputs
"""
if n_inputs != n_outputs:
raise ValueError("Input and output dimensions must match, got "
"%d inputs and %d outputs" %
(n_inputs, n_outputs))
self.n_inputs = n_inputs
self.n_outputs = n_outputs
self.n_task_dims = self.n_inputs / 3
if hasattr(self, "configuration_file"):
load_dmp_model(self, self.configuration_file)
else:
self.name = "Python DMP"
self.alpha_z = dmp.calculate_alpha(0.01, self.execution_time, 0.0)
self.widths = np.empty(self.n_features)
self.centers = np.empty(self.n_features)
dmp.initialize_rbf(self.widths, self.centers, self.execution_time,
0.0, 0.8, self.alpha_z)
self.alpha_y = 25.0
self.beta_y = self.alpha_y / 4.0
self.weights = np.zeros((self.n_features, self.n_task_dims))
if not hasattr(self, "x0"):
self.x0 = None
if not hasattr(self, "x0d"):
self.x0d = np.zeros(self.n_task_dims)
if not hasattr(self, "x0dd"):
self.x0dd = np.zeros(self.n_task_dims)
if not hasattr(self, "g"):
self.g = np.zeros(self.n_task_dims)
if not hasattr(self, "gd"):
self.gd = np.zeros(self.n_task_dims)
if not hasattr(self, "gdd"):
self.gdd = np.zeros(self.n_task_dims)
self.reset()
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 test_initialize_rbf_backward_compatibility():
widths = np.empty(10)
centers = np.empty(10)
dmp.initialize_rbf(widths, centers, 0.446, 0.0, 0.8, 4.5814764780043)
assert_array_almost_equal(
widths,
np.array([
1.39105769528669, 3.87070066903258, 10.7704545397461,
29.969429545615, 83.3917179609352, 232.042408878407,
645.671786535403, 1796.62010036397, 4999.20215246208,
4999.20215246208
]))
assert_array_almost_equal(
centers,
np.array([
1, 0.59948425031894, 0.359381366380461, 0.215443469003187,
0.129154966501487, 0.0774263682681117, 0.0464158883361271,
0.0278255940220708, 0.0166810053720003, 0.00999999999999978
]))
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)
def init(self, n_inputs, n_outputs):
"""Initialize the behavior.
Parameters
----------
n_inputs : int
number of inputs
n_outputs : int
number of outputs
"""
if n_inputs != n_outputs:
raise ValueError("Input and output dimensions must match, got "
"%d inputs and %d outputs" %
(n_inputs, n_outputs))
self.n_task_dims = n_inputs // 3
if self.execution_times is None:
self.execution_times = np.ones(self.n_dmps)
self.execution_times = np.asarray(self.execution_times)
if self.n_features is None:
self.n_features = 50 * np.ones(self.n_dmps, dtype=int)
if self.subgoals is None:
self.subgoals = [
np.zeros(self.n_task_dims) for _ in range(self.n_dmps + 1)
]
else:
self.subgoals = [np.asarray(g) for g in self.subgoals]
self.n_steps_per_dmp = (self.execution_times / self.dt).astype(int) + 1
self.n_weights_per_dmp = self.n_task_dims * self.n_features
self.subgoal_velocities = [
np.zeros(self.n_task_dims) for _ in range(self.n_dmps + 1)
]
self.n_weights = np.sum(self.n_weights_per_dmp)
self.split_steps = np.cumsum(self.n_steps_per_dmp - 1)
self.split_weights = np.cumsum(self.n_weights_per_dmp)
self.alpha_z = []
self.widths = []
self.centers = []
for i in range(self.n_dmps):
alpha_z = dmp.calculate_alpha(0.01, self.execution_times[i], 0.0)
widths = np.empty(self.n_features[i])
centers = np.empty(self.n_features[i])
dmp.initialize_rbf(widths, centers, self.execution_times[i], 0.0,
0.8, alpha_z)
self.alpha_z.append(alpha_z)
self.widths.append(widths)
self.centers.append(centers)
self.alpha_y = 25.0
self.beta_y = self.alpha_y / 4.0
if self.initial_weights is None:
self.weights = [
np.zeros((self.n_weights_per_dmp[i], self.n_task_dims))
for i in range(self.n_dmps)
]
else:
self.weights = [
w.reshape(self.n_weights_per_dmp[i], self.n_task_dims)
for w in self.initial_weights
]
self.x0 = None
self.g = None
self.reset()
def init(self, n_inputs, n_outputs):
"""Initialize the behavior.
Parameters
----------
n_inputs : int
number of inputs
n_outputs : int
number of outputs
"""
if n_inputs != 7:
raise ValueError("Number of inputs must be 7")
if n_outputs != 7:
raise ValueError("Number of outputs must be 7")
self.n_inputs = 7
self.n_outputs = 7
self.n_task_dims = 6
if hasattr(self, "configuration_file"):
load_dmp_model(self, self.configuration_file)
else:
self.name = "Python CSDMP"
self.alpha_z = dmp.calculate_alpha(0.01, self.execution_time, 0.0)
self.widths = np.empty(self.n_features)
self.centers = np.empty(self.n_features)
dmp.initialize_rbf(self.widths, self.centers, self.execution_time,
0.0, 0.8, self.alpha_z)
self.alpha_y = 25.0
self.beta_y = self.alpha_y / 4.0
self.position_weights = np.empty((self.n_features, 3))
self.orientation_weights = np.empty((self.n_features, 3))
self.weights = np.zeros((self.n_features, self.n_task_dims))
if not hasattr(self, "x0"):
self.x0 = np.zeros(3)
if not hasattr(self, "x0d"):
self.x0d = np.zeros(3)
if not hasattr(self, "x0dd"):
self.x0dd = np.zeros(3)
if not hasattr(self, "g"):
self.g = np.zeros(3)
if not hasattr(self, "gd"):
self.gd = np.zeros(3)
if not hasattr(self, "gdd"):
self.gdd = np.zeros(3)
if not hasattr(self, "q0"):
self.q0 = np.array([0.0, 1.0, 0.0, 0.0])
if not hasattr(self, "q0d"):
self.q0d = np.zeros(3)
if not hasattr(self, "q0dd"):
self.q0dd = np.zeros(3)
if not hasattr(self, "qg"):
self.qg = np.array([0.0, 1.0, 0.0, 0.0])
if not hasattr(self, "qgd"):
self.qgd = np.zeros(3)
if not hasattr(self, "qgdd"):
self.qgdd = np.zeros(3)
self.reset()
