def test_dmp_save_and_load_bug_83(): random_state = np.random.RandomState(0) dmp = DMPBehavior(execution_time=1.0, dt=0.001) dmp.init(6, 6) x0 = np.zeros(2) g = np.ones(2) dmp.set_meta_parameters(["x0", "g"], [x0, g]) w = dmp.get_params() w = random_state.randn(*w.shape) * 1000.0 dmp.set_params(w) Y, Yd, Ydd = dmp.trajectory() try: dmp.save("tmp_dmp_model2.yaml") dmp.save_config("tmp_dmp_config2.yaml") dmp = DMPBehavior(configuration_file="tmp_dmp_model2.yaml") dmp.init(6, 6) dmp.load_config("tmp_dmp_config2.yaml") finally: if os.path.exists("tmp_dmp_model2.yaml"): os.remove("tmp_dmp_model2.yaml") if os.path.exists("tmp_dmp_config2.yaml"): os.remove("tmp_dmp_config2.yaml") Y2, Yd2, Ydd2 = dmp.trajectory() assert_array_almost_equal(Y, Y2) assert_array_almost_equal(Yd, Yd2) assert_array_almost_equal(Ydd, Ydd2)
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_dmp_save_and_load(): beh_original = DMPBehavior(execution_time=0.853, dt=0.001, n_features=10) beh_original.init(3 * n_task_dims, 3 * n_task_dims) x0 = np.ones(n_task_dims) * 1.29 g = np.ones(n_task_dims) * 2.13 beh_original.set_meta_parameters(["x0", "g"], [x0, g]) xva = np.zeros(3 * n_task_dims) xva[:n_task_dims] = x0 beh_original.reset() t = 0 while beh_original.can_step(): eval_loop(beh_original, xva) if t == 0: assert_array_almost_equal(xva[:n_task_dims], x0) t += 1 assert_array_almost_equal(xva[:n_task_dims], g, decimal=3) assert_equal(t, 854) assert_equal(beh_original.get_n_params(), n_task_dims * 10) 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") xva = np.zeros(3 * n_task_dims) xva[:n_task_dims] = x0 beh_loaded.reset() t = 0 while beh_loaded.can_step(): eval_loop(beh_loaded, xva) if t == 0: assert_array_almost_equal(xva[:n_task_dims], x0) t += 1 assert_array_almost_equal(xva[:n_task_dims], g, decimal=3) assert_equal(t, 854) assert_equal(beh_loaded.get_n_params(), n_task_dims * 10)
def test_dmp_set_meta_params_before_init(): beh = DMPBehavior() x0 = np.ones(n_task_dims) * 0.43 g = np.ones(n_task_dims) * -0.21 gd = np.ones(n_task_dims) * 0.12 execution_time = 1.5 beh.set_meta_parameters(["x0", "g", "gd", "execution_time"], [x0, g, gd, execution_time]) beh.init(3 * n_task_dims, 3 * n_task_dims) xva = np.zeros(3 * n_task_dims) xva[:n_task_dims] = x0 beh.reset() t = 0 while beh.can_step(): eval_loop(beh, xva) t += 1 assert_array_almost_equal(xva[:n_task_dims], g, decimal=3) assert_array_almost_equal(xva[n_task_dims:-n_task_dims], gd, decimal=2) assert_array_almost_equal(xva[-n_task_dims:], np.zeros(n_task_dims), decimal=1) assert_equal(t, 151)
def make_joint_dmp(x0, g, execution_time, dt): ik = ExactInvKin("../data/kuka_lbr.urdf", "kuka_lbr_l_link_0", "kuka_lbr_l_link_7", 1e-5, 150) beh = DMPBehavior(execution_time, dt) mp_keys = ["x0", "g"] mp_values = [x0, g] return ik, beh, mp_keys, mp_values
def test_dmp_change_execution_time(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) beh.set_meta_parameters(["x0"], [np.ones(n_task_dims)]) X1 = beh.trajectory()[0] beh.set_meta_parameters(["execution_time"], [2.0]) X2 = beh.trajectory()[0] assert_equal(X2.shape[0], 201) assert_array_almost_equal(X1, X2[::2], decimal=3)
def test_dmp_constructor_args(): beh = DMPBehavior(execution_time=2) beh.init(3 * n_task_dims, 3 * n_task_dims) xva = np.zeros(3 * n_task_dims) beh.reset() t = 0 while beh.can_step(): eval_loop(beh, xva) t += 1 assert_equal(t, 201)
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_from_config(): beh = DMPBehavior(configuration_file=DMP_CONFIG_FILE) beh.init(18, 18) xva = np.zeros(18) beh.reset() t = 0 while beh.can_step(): eval_loop(beh, xva) t += 1 assert_equal(t, 447)
def _initial_cart_params(ik): joint_dmp = DMPBehavior(execution_time, dt) n_joints = ik.get_n_joints() joint_dmp.init(n_joints * 3, n_joints * 3) joint_dmp.set_meta_parameters(["x0", "g"], [x0, g]) Q = joint_dmp.trajectory()[0] P = np.empty((Q.shape[0], 7)) for t in range(Q.shape[0]): ik.jnt_to_cart(Q[t], P[t]) cart_dmp = CartesianDMPBehavior(execution_time, dt) cart_dmp.init(7, 7) cart_dmp.imitate(P.T[:, :, np.newaxis]) return cart_dmp.get_params()
def test_dmp_get_set_params(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) assert_equal(beh.get_n_params(), 50 * n_task_dims) params = beh.get_params() assert_array_equal(params, np.zeros(50 * n_task_dims)) random_state = np.random.RandomState(0) expected_params = random_state.randn(50 * n_task_dims) beh.set_params(expected_params) actual_params = beh.get_params() assert_array_equal(actual_params, expected_params)
def test_dmp_default_dmp(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) xva = np.zeros(3 * n_task_dims) beh.reset() t = 0 while beh.can_step(): eval_loop(beh, xva) t += 1 assert_equal(t, 101) assert_array_equal(xva[:n_task_dims], np.zeros(n_task_dims)) assert_array_equal(xva[n_task_dims:-n_task_dims], np.zeros(n_task_dims)) assert_array_equal(xva[-n_task_dims:], np.zeros(n_task_dims))
def test_dmp_more_steps_than_allowed(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) xva = np.zeros(3 * n_task_dims) beh.reset() while beh.can_step(): eval_loop(beh, xva) last_x = xva[:n_task_dims].copy() eval_loop(beh, xva) assert_array_equal(xva[:n_task_dims], last_x) assert_array_equal(xva[n_task_dims:-n_task_dims], np.zeros(n_task_dims)) assert_array_equal(xva[-n_task_dims:], np.zeros(n_task_dims))
def test_dmp_change_goal_velocity(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) beh.set_meta_parameters(["gd"], [np.ones(n_task_dims)]) xva = np.zeros(3 * n_task_dims) beh.reset() while beh.can_step(): eval_loop(beh, xva) assert_array_almost_equal(xva[:n_task_dims], np.zeros(n_task_dims), decimal=2) assert_array_almost_equal(xva[n_task_dims:-n_task_dims], np.ones(n_task_dims), decimal=2) assert_array_almost_equal(xva[-n_task_dims:], np.zeros(n_task_dims), decimal=0)
def test_dmp_change_weights(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) beh.set_params(np.ones(50 * n_task_dims)) xva = np.zeros(3 * n_task_dims) beh.reset() while beh.can_step(): eval_loop(beh, xva) assert_array_almost_equal(xva[:n_task_dims], np.zeros(n_task_dims), decimal=3) assert_array_almost_equal(xva[n_task_dims:-n_task_dims], np.zeros(n_task_dims), decimal=2) assert_array_almost_equal(xva[-n_task_dims:], np.zeros(n_task_dims), decimal=1)
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_change_goal(): beh = DMPBehavior() beh.init(3 * n_task_dims, 3 * n_task_dims) beh.set_meta_parameters(["g"], [np.ones(n_task_dims)]) xva = np.zeros(3 * n_task_dims) beh.reset() while beh.can_step(): eval_loop(beh, xva) for _ in range(30): # Convergence eval_loop(beh, xva) assert_array_almost_equal(xva[:n_task_dims], np.ones(n_task_dims), decimal=5) assert_array_almost_equal(xva[n_task_dims:-n_task_dims], np.zeros(n_task_dims), decimal=4) assert_array_almost_equal(xva[-n_task_dims:], np.zeros(n_task_dims), decimal=3)
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)
from bolero.datasets import make_minimum_jerk from bolero.representation import DMPBehavior 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$")
from bolero.environment import OptimumTrajectory from bolero.behavior_search import BlackBoxSearch from bolero.optimizer import CMAESOptimizer from bolero.representation import DMPBehavior from bolero.controller import Controller n_task_dims = 2 obstacles = [np.array([0.5, 0.5]), np.array([0.6, 0.8]), np.array([0.8, 0.6])] x0 = np.zeros(n_task_dims) g = np.ones(n_task_dims) execution_time = 1.0 dt = 0.01 n_features = 10 n_episodes = 500 beh = DMPBehavior(execution_time, dt, n_features) env = OptimumTrajectory(x0, g, execution_time, dt, obstacles, penalty_goal_dist=1.0, penalty_obstacle=1000.0, penalty_acc=1.0) opt = CMAESOptimizer(variance=100.0**2, random_state=0) bs = BlackBoxSearch(beh, opt) controller = Controller(environment=env, behavior_search=bs, n_episodes=n_episodes, record_inputs=True)
plt.figure() plt.plot(np.linspace(0, 1, X_demo.shape[1]), X_demo[0, :], 'g--', linewidth=4, 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")
def test_dmp_metaparameter_not_permitted(): beh = DMPBehavior() beh.init(3, 3) assert_raises_regexp(ValueError, "Meta parameter .* is not allowed", beh.set_meta_parameters, ["unknown"], [None])
def test_dmp_dimensions_do_not_match(): beh = DMPBehavior() assert_raises_regexp(ValueError, "Input and output dimensions must match", beh.init, 1, 2)