def test_dmp_imitate_pseudoinverse():
x0, g, execution_time, dt = np.zeros(2), np.ones(2), 1.0, 0.01
beh = DMPBehavior(execution_time, dt, 200)
beh.init(6, 6)
beh.set_meta_parameters(["x0", "g"], [x0, g])
X_demo = make_minimum_jerk(x0, g, execution_time, dt)[0]
beh.imitate(X_demo)
X = beh.trajectory()[0]
assert_array_almost_equal(X_demo.T[0], X, decimal=2)
def test_dmp_save_and_load2():
import pickle
with open("reload_test_trajectory_and_dt.pickle", "r") as f:
recorded_trajectory, dt = pickle.load(f)
artificial_trajectory = np.zeros((500, 1))
artificial_trajectory[:, 0] = np.sin(np.linspace(0, 500, 1))
trajectories = [artificial_trajectory, recorded_trajectory]
for trajectory in trajectories:
n_steps = trajectory.shape[0]
n_task_dims = trajectory.shape[1]
execution_time = dt * (n_steps - 1)
beh_original = DMPBehavior(execution_time=execution_time,
dt=dt,
n_features=20)
beh_original.init(3 * n_task_dims, 3 * n_task_dims)
x0 = trajectory[0, :]
g = trajectory[-1, :]
beh_original.set_meta_parameters(["x0", "g"], [x0, g])
X = np.zeros((n_task_dims, n_steps, 1))
X[:, :, 0] = np.swapaxes(trajectory, axis1=1, axis2=0)
beh_original.imitate(X, alpha=0.01)
xva = np.zeros(3 * n_task_dims)
xva[:n_task_dims] = x0
beh_original.reset()
imitated_trajectory = beh_original.trajectory()
try:
beh_original.save("tmp_dmp_model.yaml")
beh_original.save_config("tmp_dmp_config.yaml")
beh_loaded = DMPBehavior(configuration_file="tmp_dmp_model.yaml")
beh_loaded.init(3 * n_task_dims, 3 * n_task_dims)
beh_loaded.load_config("tmp_dmp_config.yaml")
finally:
if os.path.exists("tmp_dmp_model.yaml"):
os.remove("tmp_dmp_model.yaml")
if os.path.exists("tmp_dmp_config.yaml"):
os.remove("tmp_dmp_config.yaml")
beh_loaded.reset()
reimitated_trajectory = beh_loaded.trajectory()
assert_array_almost_equal(imitated_trajectory,
reimitated_trajectory,
decimal=4)
def test_shape_trajectory_imitate():
n_step_evaluations = range(2, 100)
for n_steps in n_step_evaluations:
n_task_dims = 1
dt = 1.0 / 60 # 60 Hertz
execution_time = dt * (n_steps - 1
) # -1 for shape(n_task_dims, n_steps)
x0, g = np.zeros(1), np.ones(1)
beh = DMPBehavior(execution_time, dt, 20)
beh.init(3, 3)
beh.set_meta_parameters(["x0", "g"], [x0, g])
X_demo = np.empty((1, n_steps, 1))
X_demo[0, :, 0] = np.linspace(0, 1, n_steps)
assert_equal(n_steps, X_demo.shape[1])
beh.imitate(X_demo, alpha=0.01)
X, Xd, Xdd = beh.trajectory()
assert_equal(X_demo[0, :].shape, X.shape)
def test_dmp_imitate_2d():
x0, g, execution_time, dt = np.zeros(2), np.ones(2), 1.0, 0.001
beh = DMPBehavior(execution_time, dt, 20)
beh.init(6, 6)
beh.set_meta_parameters(["x0", "g"], [x0, g])
X_demo = make_minimum_jerk(x0, g, execution_time, dt)[0]
# Without regularization
beh.imitate(X_demo)
X = beh.trajectory()[0]
assert_array_almost_equal(X_demo.T[0], X, decimal=2)
# With alpha > 0
beh.imitate(X_demo, alpha=1.0)
X = beh.trajectory()[0]
assert_array_almost_equal(X_demo.T[0], X, decimal=3)
# Self-imitation
beh.imitate(X.T[:, :, np.newaxis])
X2 = beh.trajectory()[0]
assert_array_almost_equal(X2, X, decimal=3)
label="Demostrated trajectory")
plt.plot(np.linspace(0, 1, X_demo.shape[1]),
2 * X_demo[0, :],
'g--',
linewidth=4,
label="Demostrated trajectory")
for ni in range(10, 50, 10):
dmp = DMPBehavior(
execution_time, dt, n_features=ni
) # Can be used to optimize the weights of a DMP with a black box optimizer.
# Only the weights of the DMP will be optimized. We will use n_features gausssians.
dmp.init(3, 3) #1*3 inputs and 1*3 outputs
dmp.set_meta_parameters(
["x0", "g"],
[x0, g
]) # Set the dmp metaparameters initial state x0 and goal state g.
dmp.imitate(X_demo) # Learn weights of the DMP from a demonstration.
plt.plot(np.linspace(0, 1, X_demo.shape[1]),
dmp.trajectory()[0],
'--',
c=generateRandomColor(),
label=str(ni) + "basis functions")
plt.plot(mp_new_high[:, 0], mp_new_high[:, 1], '--g', label='Park et al.')
plt.plot(mp_new_high[:, 0], mp_new_high[:, 2], '--g', label='Park et al.')
plt.legend(loc="upper left")
plt.show()
# Compute dmp in the original state space
ni = 10
dmp = DMPBehavior(execution_time, dt, n_features=ni) # Can be used to optimize the weights of a DMP with a black box optimizer.
# Only the weights of the DMP will be optimized. We will use n_features gausssians.
dmp.init(6,6) #1*3 inputs and 1*3 outputs
print("x0.shape", x0.shape)
print("g.shape", g.shape)
print("y_demo.shape", y_demo.shape)
x0 = np.squeeze(x0)
g = np.squeeze(g)
dmp.set_meta_parameters(["x0", "g"], [x0, g]) # Set the dmp metaparameters initial state x0 and goal state g.
y_demo = np.expand_dims(y_demo, axis=2)
dmp.imitate(y_demo) # Learn weights of the DMP from a demonstration.
trajectory_goal_demo = dmp.trajectory()
# New goal
g [0] = 10*np.pi/180
g [1] = -10*np.pi/180
dmp.set_meta_parameters(["x0", "g"], [x0, g]) # Set the dmp metaparameters initial state x0 and goal state g.
dmp.imitate(y_demo) # Learn weights of the DMP from a demonstration.
trajectory_new_goal_demo = dmp.trajectory()
# Compute dmp in the transformation state space
ni = 10
dmpNewSpace = DMPBehavior(execution_time, dt, n_features=ni) # Can be used to optimize the weights of a DMP with a black box optimizer.
# Only the weights of the DMP will be optimized. We will use n_features gausssians.
def dmp_to_trajectory(dmp, x0, g, gd, execution_time):
"""Computes trajectory generated by open-loop controlled DMP."""
dmp.set_meta_parameters(["x0", "g", "gd", "execution_time"],
[x0, g, gd, execution_time])
return dmp.trajectory()
x0 = np.zeros(2)
g = np.ones(2)
dt = 0.001
execution_time = 1.0
dmp = DMPBehavior(execution_time, dt, n_features=20)
dmp.init(6, 6)
dmp.set_meta_parameters(["x0", "g"], [x0, g])
X_demo = make_minimum_jerk(x0, g, execution_time, dt)[0]
dmp.imitate(X_demo)
plt.figure()
plt.subplots_adjust(wspace=0.3, hspace=0.6)
for gx in np.linspace(0.5, 1.5, 6):
g_new = np.array([gx, 1.0])
X, Xd, Xdd = dmp_to_trajectory(dmp, x0, g_new, np.zeros(2), 1.0)
ax = plt.subplot(321)
ax.set_title("Goal adaption")
ax.set_xlabel("$x_1$")
ax.set_ylabel("$x_2$")
ax.plot(X[:, 0], X[:, 1])
ax = plt.subplot(322)