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_promp_imitate():
x0, g, execution_time, dt = np.zeros(2), np.ones(2), 1.0, 0.001
beh = ProMPBehavior(execution_time, dt, 20)
beh.init(4, 4)
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)
# Self-imitation
beh.imitate(X.T[:, :, np.newaxis])
X2 = beh.trajectory()[0]
assert_array_almost_equal(X2, X, decimal=3)
def test_minimum_jerk_boundaries():
random_state = np.random.RandomState(1)
x0 = random_state.randn(2)
g = x0 + random_state.rand(2)
X, Xd, Xdd = make_minimum_jerk(x0, g)
assert_array_almost_equal(X[:, 0, 0], x0)
assert_array_almost_equal(Xd[:, 0, 0], np.zeros(2))
assert_array_almost_equal(Xdd[:, 0, 0], np.zeros(2))
assert_array_almost_equal(X[:, -1, 0], g)
assert_array_almost_equal(Xd[:, -1, 0], np.zeros(2))
assert_array_almost_equal(Xdd[:, -1, 0], np.zeros(2))
for d in range(X.shape[0]):
for t in range(X.shape[1]):
assert_greater_equal(X[d, t, 0], x0[d])
assert_less_equal(X[d, t, 0], g[d])
assert_raises_regexp(ValueError, "Shape .* must be equal",
make_minimum_jerk, x0, np.zeros(1))
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)
b = random.random()
g = random.random()
color = (r, g, b)
return color
x0 = np.zeros(1)
x0[0] = np.sin(0.0 * 3.5 * np.pi / 2) * 15
g = np.ones(1)
g[0] = np.sin(1.0 * 3.5 * np.pi / 2) * 15
dt = 0.001
execution_time = 1.0
X_demo = make_minimum_jerk(
x0, g, execution_time, dt
)[0] # Use make_minimum_jerk to create a demonstration trajectory of one dof.
time = np.linspace(0, execution_time, math.ceil(execution_time / dt) + 1)
X_demo = np.sin(time * 3.5 * np.pi / 2) * 15
gamma = np.transpose(np.array([time, X_demo, X_demo]))
X_demo = X_demo.reshape((1, X_demo.shape[0], 1))
print(X_demo.shape)
K = 1000
alpha = 4.0
n_dim = 3
n_bfs = 50
dt = 0.001
tol = 0.05
"""
=======================
Minimum Jerk Trajectory
=======================
An example for a minimum jerk trajectory is displayed in the following plot.
"""
print(__doc__)
import matplotlib.pyplot as plt
from bolero.datasets import make_minimum_jerk
X, Xd, Xdd = make_minimum_jerk([0], [1])
plt.figure()
plt.subplot(311)
plt.ylabel("$x$")
plt.plot(X[0, :, 0])
plt.subplot(312)
plt.ylabel("$\dot{x}$")
plt.plot(Xd[0, :, 0])
plt.subplot(313)
plt.xlabel("$t$")
plt.ylabel("$\ddot{x}$")
plt.plot(Xdd[0, :, 0])
plt.show()
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])
