def perform_new_dmp(given_traj,
initial=None,
end=None,
duration=1.0,
dt=0.001,
use_improved=True):
traj_1d = np.reshape(given_traj, np.size(given_traj))
list_traj = traj_1d.tolist()
if not initial:
initial = list_traj[0]
if not end:
end = list_traj[-1]
while (hasattr(initial, "__getitem__")):
initial = initial[0]
while (hasattr(end, "__getitem__")):
end = end[0]
traj_freq = int(duration / dt)
traj = DiscreteDMP.compute_derivatives(list_traj, traj_freq)
traj = list(traj)
dmp = DiscreteDMP(improved_version=use_improved,
reg_model=LWR(activation=0.1,
exponentially_spaced=True,
n_rfs=20))
dmp.learn_batch(traj, traj_freq)
dmp_adapt = DiscreteDMP(
improved_version=use_improved,
reg_model=dmp.lwr_model) #copy.deepcopy(dmp.lwr_model))
dmp_adapt._is_learned = True
dmp.delta_t = dt
dmp.setup(traj_1d[0], traj_1d[-1], duration)
dmp_adapt.delta_t = dt
dmp_adapt.setup(initial, end, duration)
traj_reproduced = []
traj_adapted = []
for _ in range(int(dmp.tau / dmp.delta_t)):
dmp.run_step()
dmp_adapt.run_step()
traj_reproduced.append(dmp.x)
traj_adapted.append(dmp_adapt.x)
traj_reproduced = np.array(traj_reproduced).reshape(np.size(given_traj))
print(np.size(traj_adapted))
traj_adapted = np.array(traj_adapted).reshape(np.size(given_traj))
print(np.size(traj_adapted))
return [traj_adapted, traj_reproduced]
def approx_lwr_batch(self):
model = LWR(nb_features=10)
self.make_nonlinear_batch_data()
start = time.process_time()
model.train_lwls(self.x_data, self.y_data)
print("LWR time:", time.process_time() - start)
model.plot(self.x_data, self.y_data)
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 __init__(self, transf_system, reg_model=None):
self.ts = transf_system
# state variables
# -----------------------------
self.start = 0.0
self.goal = 1.0
self.tau = 1.0
# Cannonical System
# -----------------------------
# s_min
self.cutoff = 0.001
# alpha (canonical system parameter)
self.alpha = abs(math.log(self.cutoff))
# Canonical System
self.s = 1.0 # canonical system is starting with 1.0
self.s_time = 0.0
# Attractor
# -----------------------------
# spring term
self.K = 50.0
# damping term
self.D = self.K / 4
# time steps (for the run)
self.delta_t = 0.001
# Transformation System
# ----------------------------
# current position, velocity, acceleration
self.x = 0.0
self.xd = 0.0
self.xdd = 0.0
# internal variables
''' xd not scaled by tau aka v'''
self._raw_xd = 0.0
self.f = 0
# target function input (x) and output (y)
self.target_function_input = None
self.Fd = None
# debugging (y values predicted by fitted lwr model)
self.Fp = None
# do not predict f by approximated function,
# use values of ft directly only works if duration is 1.0!!
self.use_ft = False
# create LWR model and set parameters
# -----------------------------
if not reg_model:
# default regression model
self.lwr_model = LWR(activation=0.1,
exponentially_spaced=True,
n_rfs=20)
else:
# custom regression model
self.lwr_model = reg_model
# is the DMP initialized?
self._is_learned = False
class DiscreteDMP(object):
def __init__(self, transf_system, reg_model=None):
self.ts = transf_system
# state variables
# -----------------------------
self.start = 0.0
self.goal = 1.0
self.tau = 1.0
# Cannonical System
# -----------------------------
# s_min
self.cutoff = 0.001
# alpha (canonical system parameter)
self.alpha = abs(math.log(self.cutoff))
# Canonical System
self.s = 1.0 # canonical system is starting with 1.0
self.s_time = 0.0
# Attractor
# -----------------------------
# spring term
self.K = 50.0
# damping term
self.D = self.K / 4
# time steps (for the run)
self.delta_t = 0.001
# Transformation System
# ----------------------------
# current position, velocity, acceleration
self.x = 0.0
self.xd = 0.0
self.xdd = 0.0
# internal variables
''' xd not scaled by tau aka v'''
self._raw_xd = 0.0
self.f = 0
# target function input (x) and output (y)
self.target_function_input = None
self.Fd = None
# debugging (y values predicted by fitted lwr model)
self.Fp = None
# do not predict f by approximated function,
# use values of ft directly only works if duration is 1.0!!
self.use_ft = False
# create LWR model and set parameters
# -----------------------------
if not reg_model:
# default regression model
self.lwr_model = LWR(activation=0.1,
exponentially_spaced=True,
n_rfs=20)
else:
# custom regression model
self.lwr_model = reg_model
# is the DMP initialized?
self._is_learned = False
def _predict_f(self, x):
# if nothing is learned we assume f=0.0
if not self._is_learned:
print "WARNING: NO TARGET FUNCTION LEARNED assuming f = 0.0"
return 0.0
#return self.lwr_model.predict(np.asarray([x]))[0]
return self.lwr_model.predict(x)
def _create_training_set(self, trajectory, frequency):
'''
Prepares the data set for the supervised learning
@param trajectory: list of 3-Tuples with (pos,vel,acc)
'''
# number of training samples
n_samples = len(trajectory)
# duration of the movement
duration = float(n_samples) / float(frequency)
# set tau to duration for learning
tau = duration
# initial goal and start obtained from trajectory
start = trajectory[0][0]
goal = trajectory[-1][0]
print "create training set of movement from trajectory with %i entries (%i hz) \
with duration: %f, start: %f, goal: %f" % (n_samples, frequency,
duration, start, goal)
# compute target function input (canonical system) [rollout]
# -----------------------------
# compute alpha_x such that the canonical system drops
# below the cutoff when the trajectory has finished
alpha = -(math.log(self.cutoff))
# time steps
dt = 1.0 / n_samples # delta_t for learning
time = 0.0
time_steps = np.zeros(n_samples)
for i in xrange(len(time_steps)):
time_steps[i] = math.exp(-(alpha / tau) * time)
time += dt
# compute values of target function
# -----------------------------
# the target function (transformation system solved for f, and plugged in y for x)
ft = lambda y, yd, ydd, s: self.ts.fs(self.K, self.D, y, yd, ydd,
start, goal, tau, s)
# evaluate function to get the target values for given training data
F = []
for i, d in enumerate(trajectory):
# compute f_target(y, yd, ydd) * s
#print "s ", target_function_input[i], "y ", d[0], "yd ", d[1], "ydd", d[2], " ft:", ft(d[0], d[1], d[2])
F.append(ft(d[0], d[1], d[2], time_steps[i]))
return time_steps, np.asarray(F)
def _compute_derivatives(self, pos_trajectory, frequency):
# ported from trajectory.cpp
# no f*****g idea why doing it this way - but hey, the results are better ^^
add_pos_points = 4
#add points to start
for _ in range(add_pos_points):
first_point = pos_trajectory[0]
pos_trajectory.insert(0, first_point)
# add points to the end
for _ in range(add_pos_points):
first_point = pos_trajectory[-1]
pos_trajectory.append(first_point)
# derive positions
vel_trajectory = []
for i in range(len(pos_trajectory) - 4):
vel = (pos_trajectory[i] - (8.0 * pos_trajectory[i + 1]) +
(8.0 * pos_trajectory[i + 3]) -
pos_trajectory[i + 4]) / 12.0
vel *= frequency
vel_trajectory.append(vel)
# derive velocities
acc_trajectory = []
for i in range(len(vel_trajectory) - 4):
acc = (vel_trajectory[i] - (8.0 * vel_trajectory[i + 1]) +
(8.0 * vel_trajectory[i + 3]) -
vel_trajectory[i + 4]) / 12.0
acc *= frequency
acc_trajectory.append(acc)
results = zip(pos_trajectory[4:], vel_trajectory[2:], acc_trajectory)
return results
def _step(self):
'''
runs a integration step - updates variables self.x, self.xd, self.xdd
'''
dt = self.delta_t
# integrate transformation system
# -----------------------------
# update f(s)
# debugging: use raw function output (time must be 1.0)
if self.use_ft:
print "DEBUG: using ft without approximation"
ftinp = list(self.target_function_input)
self.f = self.Fd[ftinp.index(self.s)]
else:
self.f = self._predict_f(self.s)
# calculate xdd (vd) according to the transformation system equation 1
self.xdd = self.ts.transformation(self.K, self.D, self.x, self._raw_xd,
self.start, self.goal, self.tau,
self.f, self.s)
# calculate xd using the raw_xd (scale by tau)
self.xd = (self._raw_xd / self.tau)
# integrate (update xd with xdd)
self._raw_xd += self.xdd * dt
# integrate (update x with xd)
self.x += self.xd * dt
# integrate canonical system
# -----------------------------
self.s = math.exp(-(self.alpha / self.tau) * self.s_time)
self.s_time += dt
return self.x, self.xd, self.xdd
def learn(self, trajectory, time):
'''
Learns the DMP by a given sample trajectory
@param sample_trajectory: list of tuples (pos,vel,acc)
'''
assert len(trajectory) > 0
totalSteps = int(time / self.delta_t) # frequency
if isinstance(trajectory[0], float):
trajectory = self._compute_derivatives(trajectory, totalSteps)
if len(trajectory[0]) != 3:
raise Exception(
"trajectory must be a list with 3-tuples [(1,2,3),(4,5,6)]")
# get input and output of desired target function
time_steps, Fd = self._create_training_set(trajectory, totalSteps)
# save input/output of f_target
self.target_function_input = time_steps
self.Fd = Fd
# learn LWR Model for this transformation system
self.lwr_model.learn(time_steps, Fd)
self._is_learned = True
# debugging: compute learned ft(x)
self.Fp = []
for x in time_steps:
self.Fp.append(self._predict_f(x))
def setup(self, start, goal, duration):
self.start = start
self.goal = goal
self.tau = duration
def plan(self, time):
trajectory = []
self.x = self.start
stepNumber = 0
totalSteps = int(time / self.delta_t)
while stepNumber < totalSteps:
x, xd, xdd = self._step()
trajectory.append([x, xd, xdd])
stepNumber += 1
pos = np.squeeze(np.array(np.matrix(trajectory)[:, 0]))
vel = np.squeeze(np.array(np.matrix(trajectory)[:, 1]))
acc = np.squeeze(np.array(np.matrix(trajectory)[:, 2]))
return pos, vel, acc
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 perform_new_dmp_adapted(given_traj,
initial=None,
end=None,
duration=1.0,
dt=None,
use_improved=True,
k=None,
d=None):
traj_1d = np.reshape(given_traj, np.size(given_traj))
list_traj = traj_1d.tolist()
if not initial:
initial = list_traj[0]
if not end:
end = list_traj[-1]
if (isinstance(initial, (list, np.ndarray))):
print(initial)
initial = initial[0]
if (isinstance(end, (list, np.ndarray))):
end = end[0]
traj_freq = int(np.size(given_traj))
if not dt:
dt = duration / traj_freq
traj = DiscreteDMP.compute_derivatives(list_traj, traj_freq)
traj = list(traj)
dmp = DiscreteDMP(improved_version=use_improved,
reg_model=LWR(activation=0.1,
exponentially_spaced=True,
n_rfs=20),
K=k,
D=d)
dmp.learn_batch(traj, traj_freq)
dmp_adapt = DiscreteDMP(improved_version=use_improved,
reg_model=dmp.lwr_model,
K=k,
D=d) #copy.deepcopy(dmp.lwr_model))
dmp_adapt._is_learned = True
dmp.delta_t = dt
dmp.setup(traj_1d[0], traj_1d[-1], duration)
dmp_adapt.delta_t = dt
dmp_adapt.setup(initial, end, duration)
traj_reproduced = []
traj_adapted = []
for _ in range(int(dmp.tau / dmp.delta_t)):
dmp.run_step()
dmp_adapt.run_step()
traj_reproduced.append(dmp.x)
traj_adapted.append(dmp_adapt.x)
#print(np.size(traj_adapted))
traj_adapted = np.array(traj_adapted).reshape(np.size(given_traj))
#print(np.size(traj_adapted))
#print(traj_adapted)
return traj_adapted
#def perform_dmp_improved(traj, initial=[], end=[], ay=None, by=None):
# #set up endpoints if none specified
# if not initial:
# initial = traj[0]
# if not end:
# end = traj[max(np.shape(traj)) - 1]
# #transpose if necessary
# if len(np.shape(traj)) > 1:
# if np.shape(traj)[0] > np.shape(traj)[1]:
# traj = np.transpose(traj)
# ntraj = np.reshape(traj, (1, max(np.shape(traj))))
# ## DMP ##
# dmp_traj = perform_dmp(ntraj, [initial, end], alpha_y=ay, beta_y=by)
# dmp_traj = np.reshape(dmp_traj, np.size(traj))
# return dmp_traj
class DiscreteDMP:
def __init__(self, improved_version=False, reg_model=None):
## time constants choosen for critical damping
# s_min
self.cutoff = 0.001
# alpha (canonical system parameter)
self.alpha = abs(math.log(self.cutoff))
# spring term
self.k_gain = 50.0
# damping term
self.d_gain = self.k_gain / 4
# time steps (for the run)
self.delta_t = 0.001
## state variables
'''start position aka $x_0$'''
self.start = 0.0
'''goal position aka $g$'''
self.goal = 1.0
'''movement duration: temporal scaling factor $\tau$'''
self.tau = 1.0
## Transformation System
# current position, velocity, acceleration
'''$x$ (position)'''
self.x = 0.0
'''$\dot x$ aka v (velocity)'''
self.xd = 0.0
'''$\ddot x$ aka \dot v (acceleration)'''
self.xdd = 0.0
# internal variables
''' xd not scaled by tau aka v'''
self._raw_xd = 0.0
'''current value of function f (perturbation function)'''
self.f = 0.0
# target function input (x) and output (y)
self.target_function_input = None
self.target_function_ouput = None
# debugging (y values predicted by fitted lwr model)
self.target_function_predicted = None
# do not predict f by approximated function, use values of ft directly - only works if duration is 1.0!!
self.use_ft = False
# create LWR model and set parameters
if not reg_model:
# default regression model
self.lwr_model = LWR(activation=0.1, exponentially_spaced=True, n_rfs=20)
else:
# custom regression model
self.lwr_model = reg_model
# Canonical System
self.s = 1.0 # canonical system is starting with 1.0
self.s_time = 0.0
# is the DMP initialized?
self._initialized = False
self._is_learned = False
# set the correct transformation and ftarget functions
if improved_version:
self._transformation_func = self._transformation_func_improved
self._ftarget_func = self._ftarget_func_improved
else:
self._transformation_func = self._transformation_func_original
self._ftarget_func = self._ftarget_func_original
# original formulation
@staticmethod
def _transformation_func_original(k_gain, d_gain, x, raw_xd, start, goal, tau, f, s):
return (k_gain * (goal - x) - d_gain * raw_xd + (goal - start) * f) / tau
@staticmethod
def _ftarget_func_original(k_gain, d_gain, y, yd, ydd, goal, start, tau, s):
return ((-1 * k_gain * (goal - y) + d_gain * yd + tau * ydd) / (goal - start))
# improved version of formulation
@staticmethod
def _transformation_func_improved(k_gain, d_gain, x, raw_xd, start, goal, tau, f, s):
return (k_gain * (goal - x) - d_gain * raw_xd - k_gain * (goal - start) * s + k_gain * f) / tau
@staticmethod
def _ftarget_func_improved(k_gain, d_gain, y, yd, ydd, goal, start, tau, s):
# published formula
#return ((tau * ydd - d_gain * yd) / k_gain ) + (goal - y) - ((goal - start) * s)
# version from dmp_lib/transformation_systm.cpp
return ((tau**2 * ydd + d_gain * yd * tau) / k_gain ) - (goal - y) + ((goal - start) * s)
# predict f
def predict_f(self, x):
# if nothing is learned we assume f=0.0
if not self._is_learned:
print "WARNING: NO TARGET FUNCTION LEARNED assuming f = 0.0"
return 0.0
#return self.lwr_model.predict(np.asarray([x]))[0]
return self.lwr_model.predict(x)
def setup(self, start, goal, duration):
assert not self._initialized
# set start position
self.start = start
# set current x to start (this is only a good idea if we not already started the dmp, which is the case for setup)
self.x = start
# set goal
self.goal = goal
self.tau = duration
self._initialized = True
def reset(self):
self.x = 0
self.xd = 0
self.xdd = 0
self._raw_xd = 0
self.f = 0
self.s = 1.0
self.s_time = 0.0
def _create_training_set(self, trajectory, frequency):
'''
Prepares the data set for the supervised learning
@param trajectory: list of 3-Tuples with (pos,vel,acc)
'''
# number of training samples
n_samples = len(trajectory)
# duration of the movement
duration = float(n_samples) / float(frequency)
# set tau to duration for learning
tau = duration
# initial goal and start obtained from trajectory
start = trajectory[0][0]
goal = trajectory[-1][0]
print "create training set of movement from trajectory with %i entries (%i hz) with duration: %f, start: %f, goal: %f" % (n_samples, frequency, duration, start, goal)
##### compute target function input (canonical system) [rollout]
# compute alpha_x such that the canonical system drops
# below the cutoff when the trajectory has finished
alpha = -(math.log(self.cutoff))
# time steps
dt = 1.0 / n_samples # delta_t for learning
time = 0.0
target_function_input = np.zeros(n_samples)
for i in xrange(len(target_function_input)):
target_function_input[i] = math.exp(-(alpha / tau) * time)
time += dt
# import pylab as plt
# plt.figure()
# plt.plot(target_function_input)
# plt.show()
# vectorized:
#time_steps = np.arange(0, 1.0, dt)
#target_function_input2 = np.exp(-(alpha) * time_steps)
#if (target_function_input == target_function_input2).all():
# print "same!"
# print "target_function_input",len(target_function_input)
##### compute values of target function
# the target function (transformation system solved for f, and plugged in y for x)
#ft = lambda y, yd, ydd, s: ((-(self.k_gain) * (goal - y) + self.d_gain * yd + tau * ydd) / (goal - start))
ft = lambda y, yd, ydd, s: self._ftarget_func(self.k_gain, self.d_gain, y, yd, ydd, goal, start, tau, s)
# evaluate function to get the target values for given training data
target_function_ouput = []
for i, d in enumerate(trajectory):
# compute f_target(y, yd, ydd) * s
#print "s ", target_function_input[i], "y ", d[0], "yd ", d[1], "ydd", d[2], " ft:", ft(d[0], d[1], d[2])
target_function_ouput.append(ft(d[0], d[1], d[2], target_function_input[i]))
return target_function_input, np.asarray(target_function_ouput)
@staticmethod
def compute_derivatives(pos_trajectory, frequency):
# ported from trajectory.cpp
# no f*****g idea why doing it this way - but hey, the results are better ^^
add_pos_points = 4
#add points to start
for _ in range(add_pos_points):
first_point = pos_trajectory[0]
pos_trajectory.insert(0, first_point)
# add points to the end
for _ in range(add_pos_points):
first_point = pos_trajectory[-1]
pos_trajectory.append(first_point)
# derive positions
vel_trajectory = []
for i in range(len(pos_trajectory) - 4):
vel = (pos_trajectory[i] - (8.0 * pos_trajectory[i + 1]) + (8.0 * pos_trajectory[i + 3]) - pos_trajectory[i + 4]) / 12.0
vel *= frequency
vel_trajectory.append(vel)
# derive velocities
acc_trajectory = []
for i in range(len(vel_trajectory) - 4):
acc = (vel_trajectory[i] - (8.0 * vel_trajectory[i + 1]) + (8.0 * vel_trajectory[i + 3]) - vel_trajectory[i + 4]) / 12.0
acc *= frequency
acc_trajectory.append(acc)
result_traj = zip(pos_trajectory[4:], vel_trajectory[2:], acc_trajectory)
return result_traj
def learn_batch(self, sample_trajectory, frequency):
'''
Learns the DMP by a given sample trajectory
@param sample_trajectory: list of tuples (pos,vel,acc)
'''
assert len(sample_trajectory) > 0
if isinstance(sample_trajectory[0], float):
# calculate yd and ydd if sample_trajectory does not contain it
print "automatic derivation of yd and ydd"
sample_trajectory = self.compute_derivatives(sample_trajectory, frequency)
if len(sample_trajectory[0]) != 3:
raise Exception("malformed trajectory, has to be a list with 3-tuples [(1,2,3),(4,5,6)]")
# get input and output of desired target function
target_function_input, target_function_ouput = self._create_training_set(sample_trajectory, frequency)
# save input/output of f_target
self.target_function_input = target_function_input
self.target_function_ouput = target_function_ouput
print "target_function_ouput len: ", len(target_function_ouput)
# learn LWR Model for this transformation system
self.lwr_model.learn(target_function_input, target_function_ouput)
# inM = np.asmatrix(target_function_input).T
# outM = np.asmatrix(target_function_ouput).T
# # learn lwpr model
# for i in range(len(target_function_input)):
# #print "value", outM[i]
# self.lwr_model.update(inM[i], outM[i])
#
self._is_learned = True
# debugging: compute learned ft(x)
self.target_function_predicted = []
for x in target_function_input:
self.target_function_predicted.append(self.predict_f(x))
def run_step(self):
'''
runs a integration step - updates variables self.x, self.xd, self.xdd
'''
assert self._initialized
dt = self.delta_t
### integrate transformation system
# update f(s)
# debugging: use raw function output (time must be 1.0)
if self.use_ft:
print "DEBUG: using ft without approximation"
ftinp = list(self.target_function_input)
ft = self.target_function_ouput[ftinp.index(self.s)]
self.f = ft
else:
f = self.predict_f(self.s)
self.f = f
# calculate xdd (vd) according to the transformation system equation 1
#self.xdd = (self.k_gain * (self.goal - self.x) - self.d_gain * self._raw_xd + (self.goal - self.start) * self.f) / self.tau
self.xdd = self._transformation_func(self.k_gain, self.d_gain, self.x, self._raw_xd, self.start, self.goal, self.tau, self.f, self.s)
# calculate xd using the raw_xd (scale by tau)
self.xd = (self._raw_xd / self.tau)
# integrate (update xd with xdd)
self._raw_xd += self.xdd * dt
# integrate (update x with xd)
self.x += self.xd * dt
# integrate canonical system
self.s = math.exp(-(self.alpha / self.tau) * self.s_time)
self.s_time += dt
def __init__(self, improved_version=False, reg_model=None):
## time constants choosen for critical damping
# s_min
self.cutoff = 0.001
# alpha (canonical system parameter)
self.alpha = abs(math.log(self.cutoff))
# spring term
self.k_gain = 50.0
# damping term
self.d_gain = self.k_gain / 4
# time steps (for the run)
self.delta_t = 0.001
## state variables
'''start position aka $x_0$'''
self.start = 0.0
'''goal position aka $g$'''
self.goal = 1.0
'''movement duration: temporal scaling factor $\tau$'''
self.tau = 1.0
## Transformation System
# current position, velocity, acceleration
'''$x$ (position)'''
self.x = 0.0
'''$\dot x$ aka v (velocity)'''
self.xd = 0.0
'''$\ddot x$ aka \dot v (acceleration)'''
self.xdd = 0.0
# internal variables
''' xd not scaled by tau aka v'''
self._raw_xd = 0.0
'''current value of function f (perturbation function)'''
self.f = 0.0
# target function input (x) and output (y)
self.target_function_input = None
self.target_function_ouput = None
# debugging (y values predicted by fitted lwr model)
self.target_function_predicted = None
# do not predict f by approximated function, use values of ft directly - only works if duration is 1.0!!
self.use_ft = False
# create LWR model and set parameters
if not reg_model:
# default regression model
self.lwr_model = LWR(activation=0.1, exponentially_spaced=True, n_rfs=20)
else:
# custom regression model
self.lwr_model = reg_model
# Canonical System
self.s = 1.0 # canonical system is starting with 1.0
self.s_time = 0.0
# is the DMP initialized?
self._initialized = False
self._is_learned = False
# set the correct transformation and ftarget functions
if improved_version:
self._transformation_func = self._transformation_func_improved
self._ftarget_func = self._ftarget_func_improved
else:
self._transformation_func = self._transformation_func_original
self._ftarget_func = self._ftarget_func_original
def perform_new_dmp_adapted(given_traj,
initial=None,
end=None,
duration=1.0,
dt=None,
use_improved=True,
k=None,
d=None):
traj_1d = np.reshape(given_traj, np.size(given_traj))
list_traj = traj_1d.tolist()
if not initial:
initial = list_traj[0]
if not end:
end = list_traj[-1]
if (isinstance(initial, (list, np.ndarray))):
print(initial)
initial = initial[0]
if (isinstance(end, (list, np.ndarray))):
end = end[0]
traj_freq = int(np.size(given_traj))
if not dt:
dt = duration / traj_freq
traj = DiscreteDMP.compute_derivatives(list_traj, traj_freq)
traj = list(traj)
dmp = DiscreteDMP(improved_version=use_improved,
reg_model=LWR(activation=0.1,
exponentially_spaced=True,
n_rfs=20),
K=k,
D=d)
dmp.learn_batch(traj, traj_freq)
dmp_adapt = DiscreteDMP(improved_version=use_improved,
reg_model=dmp.lwr_model,
K=k,
D=d) #copy.deepcopy(dmp.lwr_model))
dmp_adapt._is_learned = True
dmp.delta_t = dt
dmp.setup(traj_1d[0], traj_1d[-1], duration)
dmp_adapt.delta_t = dt
dmp_adapt.setup(initial, end, duration)
traj_reproduced = []
traj_adapted = []
for _ in range(int(dmp.tau / dmp.delta_t)):
dmp.run_step()
dmp_adapt.run_step()
traj_reproduced.append(dmp.x)
traj_adapted.append(dmp_adapt.x)
#print(np.size(traj_adapted))
traj_adapted = np.array(traj_adapted).reshape(np.size(given_traj))
#print(np.size(traj_adapted))
#print(traj_adapted)
return traj_adapted
