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