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])