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_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_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 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 __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 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_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_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 _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_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_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_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_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)
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)
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_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)
def test_dmp_dimensions_do_not_match():
beh = DMPBehavior()
assert_raises_regexp(ValueError, "Input and output dimensions must match",
beh.init, 1, 2)
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()
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)
e_demo_new_space.append(e)
# print(i)
print(y_demo.shape)
e_demo_new_space = np.array(e_demo_new_space)
e_demo_new_space = np.reshape(e_demo_new_space, (1001,2))
e_demo_new_space = e_demo_new_space.T
print("e_Demo:", e_demo_new_space.shape)
print(e_demo_new_space[0,:])
print(e_demo_new_space[1,:])
print(g_new_space)
print(x0_new_space)
# Compute dmp in the original state space
ni = 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(6,6) #1*3 inputs and 1*3 outputs
print("x0.shape", x0.shape)
print("g.shape", g.shape)
print("y_demo.shape", y_demo.shape)
x0 = np.squeeze(x0)
g = np.squeeze(g)
dmp.set_meta_parameters(["x0", "g"], [x0, g]) # Set the dmp metaparameters initial state x0 and goal state g.
y_demo = np.expand_dims(y_demo, axis=2)
dmp.imitate(y_demo) # Learn weights of the DMP from a demonstration.
trajectory_goal_demo = dmp.trajectory()
# New goal
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$")
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")
