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 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 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_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_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_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_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)
class DMPBehavior(BlackBoxBehavior): """Dynamical Movement Primitive. Parameters ---------- execution_time : float, optional (default: 1) Execution time of the DMP in seconds. dt : float, optional (default: 0.01) Time between successive steps in seconds. n_features : int, optional (default: 50) Number of RBF features for each dimension of the DMP. configuration_file : string, optional (default: None) Name of a configuration file that should be used to initialize the DMP. If it is set all other arguments will be ignored. """ def __init__(self, execution_time=1.0, dt=0.01, n_features=50, configuration_file=None): self.dmp = DMPBehaviorImpl(execution_time, dt, n_features, configuration_file) def init(self, n_inputs, n_outputs): """Initialize the behavior. Parameters ---------- n_inputs : int number of inputs n_outputs : int number of outputs """ self.dmp.init(3 * n_inputs, 3 * n_outputs) self.n_joints = n_inputs self.x = np.empty(3 * self.n_joints) self.x[:] = np.nan def reset(self): self.dmp.reset() self.x[:] = 0.0 def set_inputs(self, inputs): self.x[:self.n_joints] = inputs[:] def can_step(self): return self.dmp.can_step() def step(self): self.dmp.set_inputs(self.x) self.dmp.step() self.dmp.get_outputs(self.x) def get_outputs(self, outputs): outputs[:] = self.x[:self.n_joints] def get_n_params(self): return self.dmp.get_n_params() def get_params(self): return self.dmp.get_params() def set_params(self, params): self.dmp.set_params(params) def set_meta_parameters(self, keys, values): self.dmp.set_meta_parameters(keys, values) def trajectory(self): return self.dmp.trajectory()