def main():
# start and goal of the movement (1-dim)
start = 0.5
goal = 1.0
####### generate a trajectory (minimum jerk)
# duration
duration = 1.0
# time steps
delta_t = 0.001
# trajectory is a list of 3-tuples with (pos,vel,acc)
traj = min_jerk_traj(start, goal, 1.0, delta_t)
traj_freq = int(1.0 / delta_t)
####### learn DMP
dmp = DiscreteDMP(
False,
LWR(activation=0.3,
exponentially_spaced=False,
n_rfs=8,
use_offset=True))
dmp.learn_batch(traj, traj_freq)
####### learn DMP
# setup DMP with start and goal
dmp.setup(start + 0.4, goal + 0.2, duration)
# trajectory
reproduced_traj = []
# states of canonical system (plotting)
s = []
s_time = []
# run steps (for each point of the sample trajectory)
for _ in xrange(int(dmp.tau / dmp.delta_t)):
# change goal while execution
#if x == 500:
# dmp.goal = 4.0
# run a single integration step
dmp.run_step()
# remember canonical system values
s.append(dmp.s)
s_time.append(dmp.s_time)
# save reproduced trajectory
reproduced_traj.append([dmp.x, dmp.xd, dmp.xdd])
####### PLOTTING
fig_pos = plt.figure('dmp batch learn from min jerk', figsize=(7, 5))
ax_pos2 = fig_pos.add_subplot(111)
plot_time = np.arange(len(traj)) * delta_t
ax_pos2.plot(plot_time, np.asarray(traj)[:, 0], label='demonstration')
ax_pos2.plot(plot_time,
np.asarray(reproduced_traj)[:, 0],
label='adapted reproduction',
linestyle='dashed')
ax_pos2.legend(loc='upper left')
plt.show()
# create figure
fig = plt.figure('dmp batch learn from min jerk', figsize=(16, 4.6))
# create axes
ax_pos = fig.add_subplot(131)
ax_vel = fig.add_subplot(132)
ax_acc = fig.add_subplot(133)
# plot on the axes
plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc),
traj,
delta_t,
label='demonstration')
plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc),
reproduced_traj,
dmp.delta_t,
label='reproduction',
linestyle='dashed')
fig.tight_layout()
plt.show()
fig = plt.figure('ftarget', figsize=(16, 4.6))
# plot ftarget (real and predicted)
ax_ft = fig.add_subplot(131)
ax_ft.plot(dmp.target_function_input,
dmp.target_function_ouput,
linewidth=2,
label=r'$f_{target}(s)$')
ax_ft.plot(dmp.target_function_input,
dmp.target_function_predicted,
'--',
linewidth=2,
label=r'$f_{predicted}(s)$') #dashes=(3, 3)
ax_ft.set_xlabel(r'$s$')
ax_ft.set_ylabel(r'$f(s)$')
ax_ft.legend()
# kernels (lwr)
ax_kernel = fig.add_subplot(132)
dmp.lwr_model.plot_kernels(ax_kernel)
ax_kernel.set_xlabel(r'$s$')
ax_kernel.set_ylabel(r'$\psi(s)$')
# canonical system
# ax_cs = fig.add_subplot(236)
# ax_cs.plot(s_time, s)
# ax_cs.set_xlabel('Time [s]')
# ax_cs.set_ylabel(r'$s$')
# weights of kernels (w)
ax_w = fig.add_subplot(133)
ax_w.set_xlabel(r'$s$')
ax_w.set_ylabel(r'$f(s)$')
dmp.lwr_model.plot_linear_models(ax_w)
# lwr_wights = dmp.lwr_model.get_thetas()
# ax_w.bar(range(len(lwr_wights)), lwr_wights, width=0.3)
# ax_w.set_xlabel('$w_i$')
# ax_w.set_ylabel('')
fig.tight_layout()
plt.show()
def main():
# start and goal of the movement (1-dim)
start = 0.5
goal = 1.0
####### generate a trajectory (minimum jerk)
# duration
duration = 1.0
# time steps
delta_t = 0.001
# trajectory is a list of 3-tuples with (pos,vel,acc)
traj = min_jerk_traj(start, goal, 1.0, delta_t)
traj_freq = int(1.0 / delta_t)
####### learn DMP
dmp = DiscreteDMP(False, LWR(activation=0.3, exponentially_spaced=False, n_rfs=8, use_offset=True))
dmp.learn_batch(traj, traj_freq)
####### learn DMP
# setup DMP with start and goal
dmp.setup(start+0.4, goal+0.2, duration)
# trajectory
reproduced_traj = []
# states of canonical system (plotting)
s = []
s_time = []
# run steps (for each point of the sample trajectory)
for _ in xrange(int(dmp.tau / dmp.delta_t)):
# change goal while execution
#if x == 500:
# dmp.goal = 4.0
# run a single integration step
dmp.run_step()
# remember canonical system values
s.append(dmp.s)
s_time.append(dmp.s_time)
# save reproduced trajectory
reproduced_traj.append([dmp.x, dmp.xd, dmp.xdd])
####### PLOTTING
fig_pos = plt.figure('dmp batch learn from min jerk', figsize=(7, 5))
ax_pos2 = fig_pos.add_subplot(111)
plot_time = np.arange(len(traj)) * delta_t
ax_pos2.plot(plot_time, np.asarray(traj)[:, 0], label='demonstration')
ax_pos2.plot(plot_time, np.asarray(reproduced_traj)[:, 0], label='adapted reproduction', linestyle='dashed')
ax_pos2.legend(loc='upper left')
plt.show()
# create figure
fig = plt.figure('dmp batch learn from min jerk', figsize=(16, 4.6))
# create axes
ax_pos = fig.add_subplot(131)
ax_vel = fig.add_subplot(132)
ax_acc = fig.add_subplot(133)
# plot on the axes
plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj, delta_t, label='demonstration')
plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), reproduced_traj, dmp.delta_t, label='reproduction', linestyle='dashed')
fig.tight_layout()
plt.show()
fig = plt.figure('ftarget', figsize=(16, 4.6))
# plot ftarget (real and predicted)
ax_ft = fig.add_subplot(131)
ax_ft.plot(dmp.target_function_input, dmp.target_function_ouput, linewidth=2, label=r'$f_{target}(s)$')
ax_ft.plot(dmp.target_function_input, dmp.target_function_predicted, '--', linewidth=2, label=r'$f_{predicted}(s)$') #dashes=(3, 3)
ax_ft.set_xlabel(r'$s$')
ax_ft.set_ylabel(r'$f(s)$')
ax_ft.legend()
# kernels (lwr)
ax_kernel = fig.add_subplot(132)
dmp.lwr_model.plot_kernels(ax_kernel)
ax_kernel.set_xlabel(r'$s$')
ax_kernel.set_ylabel(r'$\psi(s)$')
# canonical system
# ax_cs = fig.add_subplot(236)
# ax_cs.plot(s_time, s)
# ax_cs.set_xlabel('Time [s]')
# ax_cs.set_ylabel(r'$s$')
# weights of kernels (w)
ax_w = fig.add_subplot(133)
ax_w.set_xlabel(r'$s$')
ax_w.set_ylabel(r'$f(s)$')
dmp.lwr_model.plot_linear_models(ax_w)
# lwr_wights = dmp.lwr_model.get_thetas()
# ax_w.bar(range(len(lwr_wights)), lwr_wights, width=0.3)
# ax_w.set_xlabel('$w_i$')
# ax_w.set_ylabel('')
fig.tight_layout()
plt.show()
def main():
# duration
duration = 1.0
# adaption offset
adapt_offset = +0.02
# time steps
delta_t = 0.001
# load position trajectory
traj_pos = json.load(open("traj_full.json", 'r'))["x"][2000:8000][::6] #[5000:8000][::3]
# rest start and goal position out of trajectory
start = traj_pos[0]
goal = traj_pos[-1]
traj_freq = int(1.0 / delta_t)
# calc it here for easier drawing
traj = DiscreteDMP.compute_derivatives(traj_pos, traj_freq)
print(type(traj))
traj = list(traj)
####### learn DMP
dmp = DiscreteDMP(True, reg_model=LWR(activation=0.1, exponentially_spaced=True, n_rfs=20))
#dmp.use_ft = True
dmp.learn_batch(traj, traj_freq)
dmp_adapt = DiscreteDMP(True, reg_model=dmp.lwr_model) #copy.deepcopy(dmp.lwr_model))
dmp_adapt._is_learned = True
####### learn DMP
# setup DMP with start and goal
dmp.delta_t = delta_t
dmp.setup(start, goal, duration)
dmp_adapt.delta_t = delta_t
dmp_adapt.setup(start, goal + adapt_offset, duration)
# trajectory
traj_reproduced = []
traj_adapted = []
# states of canonical system (plotting)
s = []
s_time = []
# run steps (for each point of the sample trajectory)
for _ in range(int(dmp.tau / dmp.delta_t)):
# change goal while execution
#if x == 500:
# dmp.goal = 4.0
# run a single integration step
dmp.run_step()
dmp_adapt.run_step()
# remember canonical system values
s.append(dmp.s)
s_time.append(dmp.s_time)
# save reproduced trajectory
traj_reproduced.append([dmp.x, dmp.xd, dmp.xdd])
traj_adapted.append([dmp_adapt.x, dmp_adapt.xd, dmp_adapt.xdd])
####### PLOTTING
# create figure
fig = plt.figure('dmp', figsize=(6, 4))
# create axes
ax_pos = fig.add_subplot(111)
# plot on the axes
#ax_pos.plot(np.arange(0,1,0.001), traj_pos)
plot_time = np.arange(len(traj)) * delta_t
ax_pos.plot(plot_time, np.asarray(traj)[:, 0], label='demonstration')
ax_pos.plot(plot_time, np.asarray(traj_reproduced)[:, 0], label='reproduction', linestyle='dashed')
ax_pos.plot(plot_time, np.asarray(traj_adapted)[:, 0], label='adapted (%+0.2f)' % adapt_offset)
ax_pos.legend(loc='lower left')
# plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj, dmp.delta_t, label='demonstration')
# plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj_reproduced, dmp.delta_t, label='reproduction', linestyle='dashed')
# plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj_adapted, dmp.delta_t, label='adapted (+0.02)')
fig.tight_layout()
plt.show()
#
fig2 = plt.figure('lwr', figsize=(6, 4))
#
ax_ft = fig2.add_subplot(211)
ax_ft.plot(dmp.target_function_input, dmp.target_function_ouput, linewidth=2, label=r'$f_{target}(s)$')
ax_ft.plot(dmp.target_function_input, dmp.target_function_predicted, '--', linewidth=2, label=r'$f_{predicted}(s)$') #dashes=(3, 3)
plt.show()
def main():
# duration
duration = 1.0
# adaption offset
adapt_offset = +0.02
# time steps
delta_t = 0.001
# load position trajectory
traj_pos = json.load(open("traj_full.json", "r"))["x"][2000:8000][::6] # [5000:8000][::3]
# rest start and goal position out of trajectory
start = traj_pos[0]
goal = traj_pos[-1]
traj_freq = int(1.0 / delta_t)
# calc it here for easier drawing
traj = DiscreteDMP.compute_derivatives(traj_pos, traj_freq)
####### learn DMP
dmp = DiscreteDMP(True, reg_model=LWR(activation=0.1, exponentially_spaced=True, n_rfs=20))
# dmp.use_ft = True
dmp.learn_batch(traj, traj_freq)
dmp_adapt = DiscreteDMP(True, reg_model=dmp.lwr_model) # copy.deepcopy(dmp.lwr_model))
dmp_adapt._is_learned = True
####### learn DMP
# setup DMP with start and goal
dmp.delta_t = delta_t
dmp.setup(start, goal, duration)
dmp_adapt.delta_t = delta_t
dmp_adapt.setup(start, goal + adapt_offset, duration)
# trajectory
traj_reproduced = []
traj_adapted = []
# states of canonical system (plotting)
s = []
s_time = []
# run steps (for each point of the sample trajectory)
for _ in xrange(int(dmp.tau / dmp.delta_t)):
# change goal while execution
# if x == 500:
# dmp.goal = 4.0
# run a single integration step
dmp.run_step()
dmp_adapt.run_step()
# remember canonical system values
s.append(dmp.s)
s_time.append(dmp.s_time)
# save reproduced trajectory
traj_reproduced.append([dmp.x, dmp.xd, dmp.xdd])
traj_adapted.append([dmp_adapt.x, dmp_adapt.xd, dmp_adapt.xdd])
####### PLOTTING
# create figure
fig = plt.figure("dmp", figsize=(6, 4))
# create axes
ax_pos = fig.add_subplot(111)
# plot on the axes
# ax_pos.plot(np.arange(0,1,0.001), traj_pos)
plot_time = np.arange(len(traj)) * delta_t
ax_pos.plot(plot_time, np.asarray(traj)[:, 0], label="demonstration")
ax_pos.plot(plot_time, np.asarray(traj_reproduced)[:, 0], label="reproduction", linestyle="dashed")
ax_pos.plot(plot_time, np.asarray(traj_adapted)[:, 0], label="adapted (%+0.2f)" % adapt_offset)
ax_pos.legend(loc="lower left")
# plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj, dmp.delta_t, label='demonstration')
# plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj_reproduced, dmp.delta_t, label='reproduction', linestyle='dashed')
# plot_pos_vel_acc_trajectory((ax_pos, ax_vel, ax_acc), traj_adapted, dmp.delta_t, label='adapted (+0.02)')
fig.tight_layout()
plt.show()
#
fig2 = plt.figure("lwr", figsize=(6, 4))
#
ax_ft = fig2.add_subplot(211)
ax_ft.plot(dmp.target_function_input, dmp.target_function_ouput, linewidth=2, label=r"$f_{target}(s)$")
ax_ft.plot(
dmp.target_function_input, dmp.target_function_predicted, "--", linewidth=2, label=r"$f_{predicted}(s)$"
) # dashes=(3, 3)
plt.show()
