class DMPs(object):
"""Implementation of Dynamic Motor Primitives,
as described in Dr. Stefan Schaal's (2002) paper."""
def __init__(self, n_dmps, n_bfs, dt=.01,
y0=0, goal=1, w=None,
ay=None, by=None, axis_to_not_mirror=None,
axis_not_to_scale=None, **kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
axis_to_not_mirror: an integer indicating the axis to
not mirror. This is typically the height, for robotics
applications. If we follow the usual convention for
cartesian axis order then 0 is x, 1 is y, 2 is z and
so on, so the height its usually 1 for 2D or 2 for 3D.
axis_not_to_scale: an integer defining the axis for which
scale will not be implemented. This is to avoid
zero response when start and end positions are
exactly the same.
"""
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps)*y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps)*goal
self.goal = goal
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# Here we set the flag for the axis that will not
# be flipped (we will apply a different scalling factor
# to this axis later)
self.axis_to_not_mirror = axis_to_not_mirror
# This is the axis for which we will not compute scale
self.axis_not_to_scale = axis_not_to_scale
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# We will need this for the special non-mirroring scale
# factor later
self.cs_rollout = self.cs.rollout()
self.cs.reset_state()
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des, plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [n_dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.n_dmps))
# find the force required to move along this trajectory
for d in range(self.n_dmps):
D = self.ay[d] # These are some auxiliary variables
K = D * self.by[d] # added for clarity
if d is self.axis_to_not_mirror: # non-mirroring scalling
f_target[:, d] = (ddy_des[d] + D*dy_des[d])/K \
- (self.goal[d] - y_des[d]) \
+ (self.goal[d] - self.y0[d]) * self.cs_rollout
elif d is self.axis_not_to_scale: # without scale
f_target[:, d] = (ddy_des[d] - K * (self.goal[d] - y_des[d]) +
D*dy_des[d])
else: # original scalling
f_target[:, d] = (ddy_des[d] - K * (self.goal[d] - y_des[d]) +
D*dy_des[d])/(self.goal[d] - self.y0[d])
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title('basis functions')
# plot the desired forcing function vs approx
plt.subplot(212)
plt.plot(f_target[:,0])
plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt)
plt.legend(['f_target', 'w*psi'])
plt.title('DMP forcing function')
plt.tight_layout()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if 'tau' in kwargs:
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.n_dmps):
# generate the forcing term
f = (self.gen_front_term(x, d) *
(np.dot(psi, self.w[d])) / np.sum(psi))
# DMP acceleration
D = self.ay[d]
K = D * self.by[d]
if d is self.axis_to_not_mirror:
self.ddy[d] = (K * (self.goal[d] - self.y[d]) \
- D * self.dy[d]/tau \
- K * (self.goal[d] - self.y0[d]) * x \
+ K * f) * tau
elif d is self.axis_not_to_scale:
self.ddy[d] = (K * (self.goal[d] - self.y[d])
- D * self.dy[d]/tau
+ f) * tau
else:
self.ddy[d] = (K * (self.goal[d] - self.y[d])
- D * self.dy[d]/tau
+ f * (self.goal[d] - self.y0[d])) * tau
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * self.dt * error_coupling
return self.y, self.dy, self.ddy
class DMPs(object):
"""Implementation of Dynamic Motor Primitives,
as described in Dr. Stefan Schaal's (2002) paper."""
def __init__(self,
n_dmps,
n_bfs,
dt=.001,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
"""
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.gain_a = np.ones(n_dmps) * 25. if ay is None else np.array(
ay) # Schaal 2012
self.gain_b = self.gain_a / 4. if by is None else np.array(
by) # Schaal 2012 np.sqrt(self.gain_a)*2
# set up the CS
self.canonical_system = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.canonical_system.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des, plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [n_dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy().astype(np.float)
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
print("self.timesteps", self.timesteps)
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.canonical_system.run_time, y_des.shape[1])
#Normalizing the input trajectory in time (squashing it from [0-T] to [0-1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.n_dmps))
# find the force required to move along this trajectory
x_track = self.canonical_system.rollout(
) #(tau=1/(len(y_des[0])*self.dt))
print("track size", len(x_track))
for d in range(self.n_dmps):
f_target[:, d] = ddy_des[d]/(self.gain_a[d]*self.gain_b[d]) \
- (float(self.goal[d]) - y_des[d]) \
+ dy_des[d]/self.gain_b[d] \
+ (float(self.goal[d]) - self.y0[d])*x_track
#(ddy_des[d] - self.gain_a[d] *
# (self.gain_b[d] * (float(self.goal[d]) - y_des[d]) -
# dy_des[d]))
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.canonical_system.rollout())
print(psi_track.shape)
plt.plot(psi_track)
plt.title('basis functions')
# plot the desired forcing function vs approx
plt.subplot(212)
plt.plot(f_target[:, 0])
plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt)
plt.legend(['f_target', 'w*psi'])
plt.title('DMP forcing function')
plt.tight_layout()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if 'tau' in kwargs:
print("kwargs['tau']", kwargs['tau'])
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
print(timesteps, "timesteps")
print("goal", self.goal)
print("y0", self.y0)
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
print("dy_track", dy_track[t])
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.canonical_system.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.0
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.canonical_system.step(tau=tau, error_coupling=error_coupling)
#print("x : ", x)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.n_dmps):
# generate the forcing term
#print("x : ",x)
#print("d : ",d)
#print("self.gen_front_term(x, d) : ",self.gen_front_term(x, d))
#print("psi : ",psi)
#print("self.w[d] : ",self.w[d])
#print("(np.dot(psi, self.w[d])) : ",(np.dot(psi, self.w[d])))
#print("np.sum(psi) : ",np.sum(psi))
f = (x * (np.dot(psi, self.w[d])) / np.sum(psi)) #changed
#print("f :",f)
# DMP acceleration
#print("self.gain_a[d] : ",self.gain_a[d])
#print("self.gain_b[d] : ", self.gain_b[d])
#print("tau : ",tau)
self.ddy[d] = (
tau * tau * self.gain_a[d] *
(self.gain_b[d] *
(self.goal[d] - self.y[d] -
(self.goal[d] - self.y0[d]) * x + f) - self.dy[d] / tau)) #
#print("self.ddy[d] :",self.ddy[d])
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * self.dt * error_coupling
#print(" self.dy[d] : ", self.dy[d])
self.y[d] += self.dy[d] * self.dt * error_coupling
#print("self.y[d] :", self.y[d])
return self.y, self.dy, self.ddy
class DMPs(object):
"""Implementation of Dynamic Motor Primitives,
as described in Dr. Stefan Schaal's (2002) paper."""
def __init__(
self, n_dmps, n_bfs, dt=0.01, y0=0, goal=1, w=None, ay=None, by=None, **kwargs
):
"""
n_dmps int: number of dynamic motor primitives
n_bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
"""
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25.0 if ay is None else ay # Schaal 2012
self.by = self.ay / 4.0 if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if abs(self.y0[d] - self.goal[d]) < 1e-4:
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des, plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [n_dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
# self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des with central differences
dy_des = np.gradient(y_des, axis=1) / self.dt
# calculate acceleration of y_des with central differences
ddy_des = np.gradient(dy_des, axis=1) / self.dt
f_target = np.zeros((y_des.shape[1], self.n_dmps))
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, d] = ddy_des[d] - self.ay[d] * (
self.by[d] * (self.goal[d] - y_des[d]) - dy_des[d]
)
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title("basis functions")
# plot the desired forcing function vs approx
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
plt.plot(f_target[:, ii], "--", label="f_target %i" % ii)
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
print("w shape: ", self.w.shape)
plt.plot(
np.sum(psi_track * self.w[ii], axis=1) * self.dt,
label="w*psi %i" % ii,
)
plt.legend()
plt.title("DMP forcing function")
plt.tight_layout()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, custom_start = None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if custom_start is not None:
self.y = np.asarray(custom_start).copy()
assert (self.y.shape == self.dy.shape, "Dimensions of custom goal \
provided does not match the dimensions of the DMP")
if timesteps is None:
if "tau" in kwargs:
timesteps = int(self.timesteps / kwargs["tau"])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.n_dmps):
# generate the forcing term
f = self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) / np.sum(psi)
# DMP acceleration
self.ddy[d] = (
self.ay[d] * (self.by[d] * (self.goal[d] - self.y[d]) - self.dy[d]) + f
)
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * tau * self.dt * error_coupling
return self.y, self.dy, self.ddy
class DMPs(object):
"""Implementation of Dynamic Motor Primitives,
as described in Dr. Stefan Schaal's (2002) paper."""
def __init__(self,
n_dmps,
n_bfs,
dt=.01,
y0=0,
goal=1,
w=None,
ay=None,
by=None,
**kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
"""
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des, plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [n_dmps, run_time]
modified by lagrassa to take in a list of y_des of form [n_trajs, n_dmps, run_time]
"""
# set initial state and goal
self.n_trajs = y_des.shape[0]
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, :, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
#self.check_offset() #disabling for multi, hopefully our starts won't be too close to the goal
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_trajs, self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[-1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[:, d, :])
for t in range(self.timesteps):
path[:, d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.concatenate((np.zeros(
(self.n_trajs, self.n_dmps, 1)), dy_des),
axis=2)
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.concatenate((np.zeros(
(self.n_trajs, self.n_dmps, 1)), ddy_des),
axis=2)
f_target = np.zeros((self.n_trajs, y_des.shape[-1], self.n_dmps))
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, :, d] = (
ddy_des[:, d, :] - self.ay[d] *
(self.by[d] *
((np.ones(y_des[:, d, :].T.shape) * self.goal[:, d]).T -
y_des[:, d, :]) - dy_des[:, d, :]))
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title('basis functions')
# plot the desired forcing function vs approx
plt.subplot(212)
plt.plot(f_target[:, 0])
plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt)
plt.legend(['f_target', 'w*psi'])
plt.title('DMP forcing function')
plt.tight_layout()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if 'tau' in kwargs:
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def reset_state(self, y0=None, goal=None):
"""Reset the system state"""
if y0 is not None:
self.y0 = y0
if goal is not None:
self.goal = goal
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.n_dmps):
# generate the forcing term
f = (self.gen_front_term(x, d) * (np.dot(psi, self.w[d])) /
np.sum(psi))
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] *
(self.goal[d] - self.y[d]) - self.dy[d] / tau) +
f) * tau
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * self.dt * error_coupling
return self.y, self.dy, self.ddy
class DMPs(object):
"""Implementation of Dynamic Motor Primitives,
as described in Dr. Stefan Schaal's (2002) paper."""
def __init__(self, n_dmps, n_bfs, dt=.01,
y0=0, goal=1, w=None,
ay=None, by=None, **kwargs):
"""
n_dmps int: number of dynamic motor primitives
n_bfs int: number of basis functions per DMP
dt float: timestep for simulation
y0 list: initial state of DMPs
goal list: goal state of DMPs
w list: tunable parameters, control amplitude of basis functions
ay int: gain on attractor term y dynamics
by int: gain on attractor term y dynamics
"""
self.n_dmps = n_dmps
self.n_bfs = n_bfs
self.dt = dt
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps)*y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps)*goal
self.goal = goal
if w is None:
# default is f = 0
w = np.zeros((self.n_dmps, self.n_bfs))
self.w = w
self.ay = np.ones(n_dmps) * 25. if ay is None else ay # Schaal 2012
self.by = self.ay / 4. if by is None else by # Schaal 2012
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
# set up the DMP system
self.reset_state()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if (self.y0[d] == self.goal[d]):
self.goal[d] += 1e-4
def gen_front_term(self, x, dmp_num):
raise NotImplementedError()
def gen_goal(self, y_des):
raise NotImplementedError()
def gen_psi(self):
raise NotImplementedError()
def gen_weights(self, f_target):
raise NotImplementedError()
def imitate_path(self, y_des, plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [n_dmps, run_time]
"""
# set initial state and goal
if y_des.ndim == 1:
y_des = y_des.reshape(1, len(y_des))
self.y0 = y_des[:, 0].copy()
self.y_des = y_des.copy()
self.goal = self.gen_goal(y_des)
self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
path = np.zeros((self.n_dmps, self.timesteps))
x = np.linspace(0, self.cs.run_time, y_des.shape[1])
for d in range(self.n_dmps):
path_gen = scipy.interpolate.interp1d(x, y_des[d])
for t in range(self.timesteps):
path[d, t] = path_gen(t * self.dt)
y_des = path
# calculate velocity of y_des
dy_des = np.diff(y_des) / self.dt
# add zero to the beginning of every row
dy_des = np.hstack((np.zeros((self.n_dmps, 1)), dy_des))
# calculate acceleration of y_des
ddy_des = np.diff(dy_des) / self.dt
# add zero to the beginning of every row
ddy_des = np.hstack((np.zeros((self.n_dmps, 1)), ddy_des))
f_target = np.zeros((y_des.shape[1], self.n_dmps))
# find the force required to move along this trajectory
for d in range(self.n_dmps):
f_target[:, d] = (ddy_des[d] - self.ay[d] *
(self.by[d] * (self.goal[d] - y_des[d]) -
dy_des[d]))
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(psi_track)
plt.title('basis functions')
# plot the desired forcing function vs approx
plt.subplot(212)
plt.plot(f_target[:,0])
plt.plot(np.sum(psi_track * self.w[0], axis=1) * self.dt)
plt.legend(['f_target', 'w*psi'])
plt.title('DMP forcing function')
plt.tight_layout()
plt.show()
self.reset_state()
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if 'tau' in kwargs:
timesteps = int(self.timesteps / kwargs['tau'])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
for d in range(self.n_dmps):
# generate the forcing term
f = (self.gen_front_term(x, d) *
(np.dot(psi, self.w[d])) / np.sum(psi))
# DMP acceleration
self.ddy[d] = (self.ay[d] *
(self.by[d] * (self.goal[d] - self.y[d]) -
self.dy[d]/tau) + f) * tau
if external_force is not None:
self.ddy[d] += external_force[d]
self.dy[d] += self.ddy[d] * tau * self.dt * error_coupling
self.y[d] += self.dy[d] * self.dt * error_coupling
return self.y, self.dy, self.ddy
class DMPs_discrete_modified(object):
"""
An implementation of Cartesian discrete DMPs
Modified based on DMP++:
https://github.com/mginesi/dmp_pp
Using quaternion (w,i,j,k) DMPs for orientation in Cartesian space.
"""
def __init__(self,
n_dmps,
n_bfs,
dt=0.01,
ax=4.0,
y0=0,
goal=1,
w=None,
K=1050,
D=None,
form='mod',
basis='gaussian',
rescale='rotodilatation',
dim='position',
**kwargs):
"""
"""
self.n_dmps = copy.deepcopy(n_dmps)
self.n_bfs = n_bfs
self.dt = copy.deepcopy(dt)
self.ax = copy.deepcopy(ax)
self.form = copy.deepcopy(form)
self.rescale = copy.deepcopy(rescale)
self.basis = copy.deepcopy(basis)
self.dim = copy.deepcopy(dim)
if isinstance(y0, (int, float)):
y0 = np.ones(self.n_dmps) * y0
self.y0 = y0
if isinstance(goal, (int, float)):
goal = np.ones(self.n_dmps) * goal
self.goal = goal
if w is None:
w = np.zeros((self.n_dmps, self.n_bfs)) if self.dim == 'position' \
else np.zeros((self.n_dmps-1, self.n_bfs))
self.w = copy.deepcopy(w)
self.K = copy.deepcopy(K)
if D is None:
D = 2 * np.sqrt(self.K)
self.D = copy.deepcopy(D)
# set up the CS
self.cs = CanonicalSystem(dt=self.dt, ax=self.ax, **kwargs)
self.timesteps = int(self.cs.run_time / self.dt)
self.gen_centers()
# set variance of Gaussian basis functions
# trial and error to find this spacing
self.gen_width()
# set up the DMP system
self.reset_state()
# self.h = np.ones(self.n_bfs) * self.n_bfs ** 1.5 / self.c / self.cs.ax
# self.check_offset()
def check_offset(self):
"""Check to see if initial position and goal are the same
if they are, offset slightly so that the forcing term is not 0"""
for d in range(self.n_dmps):
if abs(self.y0[d] - self.goal[d]) < 1e-4:
self.goal[d] += 1e-4
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if "tau" in kwargs:
timesteps = int(self.timesteps / kwargs["tau"])
else:
timesteps = self.timesteps
# set up tracking vectors
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def gen_width(self):
'''
Set the "widths" for the basis functions.
'''
if self.basis == 'gaussian':
self.h = 1.0 / np.diff(self.c) / np.diff(self.c)
self.h = np.append(self.h, self.h[-1])
else:
self.h = 0.2 / np.diff(self.c)
self.h = np.append(self.h[0], self.h)
def gen_centers(self):
"""Set the centre of the Gaussian basis
functions be spaced evenly throughout run time"""
"""x_track = self.cs.discrete_rollout()
t = np.arange(len(x_track))*self.dt
# choose the points in time we'd like centers to be at
c_des = np.linspace(0, self.cs.run_time, self.n_bfs)
self.c = np.zeros(len(c_des))
for ii, point in enumerate(c_des):
diff = abs(t - point)
self.c[ii] = x_track[np.where(diff == min(diff))[0][0]]"""
# desired activations throughout time
self.c = np.exp(
-self.cs.ax * self.cs.run_time *
((np.cumsum(np.ones([1, self.n_bfs])) - 1) / self.n_bfs))
def gen_front_term(self, x, n):
"""Generates the diminishing front term on
the forcing term.
x float: the current value of the canonical system
dmp_num int: the index of the current dmp
"""
if self.dim == 'position':
return x * (self.goal - self.y0)
if self.dim == 'orientation':
return x * q_oper.quaternion_error(self.goal, self.y0)
def gen_goal(self, y_des):
"""Generate the goal for path imitation.
For rhythmic DMPs the goal is the average of the
desired trajectory.
y_des np.array: the desired trajectory to follow
"""
return np.copy(y_des[-1])
def gen_psi(self, x):
"""Generates the activity of the basis functions for a given
canonical system rollout.
x float, array: the canonical system state or path
"""
c = np.reshape(self.c, [self.n_bfs, 1])
h = np.reshape(self.h, [self.n_bfs, 1])
if self.basis == 'gaussian':
xi = h * (x - c)**2
psi_set = np.exp(-xi)
else:
xi = np.abs(h * (x - c))
if self.basis == 'mollifier':
psi_set = (np.exp(-1.0 / (1.0 - xi * xi))) * (xi < 1.0)
elif self.basis == 'wendland2':
psi_set = ((1.0 - xi)**2.0) * (xi < 1.0)
elif self.basis == 'wendland3':
psi_set = ((1.0 - xi)**3.0) * (xi < 1.0)
elif self.basis == 'wendland4':
psi_set = ((1.0 - xi)**4.0 * (4.0 * xi + 1.0)) * (xi < 1.0)
elif self.basis == 'wendland5':
psi_set = ((1.0 - xi)**5.0 * (5.0 * xi + 1)) * (xi < 1.0)
elif self.basis == 'wendland6':
psi_set = ((1.0 - xi)**6.0 *
(35.0 * xi**2.0 + 18.0 * xi + 3.0)) * (xi < 1.0)
elif self.basis == 'wendland7':
psi_set = ((1.0 - xi)**7.0 *
(16.0 * xi**2.0 + 7.0 * xi + 1.0)) * (xi < 1.0)
elif self.basis == 'wendland8':
psi_set = (
((1.0 - xi)**8.0 *
(32.0 * xi**3.0 + 25.0 * xi**2.0 + 8.0 * xi + 1.0)) *
(xi < 1.0))
psi_set = np.nan_to_num(psi_set)
return psi_set
def gen_weights(self, f_target):
"""Generate a set of weights over the basis functions such
that the target forcing term trajectory is matched.
f_target np.array: the desired forcing term trajectory
"""
# calculate x and psi
# f_target = np.transpose(f_target)
s_track = self.cs.rollout()
psi_track = self.gen_psi(s_track)
# Compute useful quantities
sum_psi = np.sum(psi_track, 0)
sum_psi_2 = sum_psi * sum_psi
s_track_2 = s_track * s_track
# Set up the minimization problem
A = np.zeros([self.n_bfs, self.n_bfs])
b = np.zeros([self.n_bfs])
# The matrix does not depend on f
for k in range(self.n_bfs):
A[k, k] = scipy.integrate.simps(
psi_track[k] * psi_track[k] * s_track_2 / sum_psi_2, s_track)
for h in range(k + 1, self.n_bfs):
A[k, h] = scipy.integrate.simps(
psi_track[k] * psi_track[h] * s_track_2 / sum_psi_2,
s_track)
A[h, k] = A[k, h].copy()
LU = scipy.linalg.lu_factor(A)
# The problem is decoupled for each dimension
if self.dim == 'position':
for d in range(self.n_dmps):
# Create the vector of the regression problem
for k in range(self.n_bfs):
b[k] = scipy.integrate.simps(
f_target[d] * psi_track[k] * s_track / sum_psi,
s_track)
# Solve the minimization problem
self.w[d] = scipy.linalg.lu_solve(LU, b)
else:
for d in range(self.n_dmps - 1):
# Create the vector of the regression problem
for k in range(self.n_bfs):
b[k] = scipy.integrate.simps(
f_target[d] * psi_track[k] * s_track / sum_psi,
s_track)
# Solve the minimization problem
self.w[d] = scipy.linalg.lu_solve(LU, b)
self.w = np.nan_to_num(self.w)
def imitate_path(self,
y_des,
dy_des=None,
ddy_des=None,
t_des=None,
plot=False):
"""Takes in a desired trajectory and generates the set of
system parameters that best realize this path.
y_des list/array: the desired trajectories of each DMP
should be shaped [run_time, n_dmps]
"""
# set initial state and goal
self.y0 = y_des[0].copy()
self.goal = y_des[-1].copy()
# self.check_offset()
# generate function to interpolate the desired trajectory
import scipy.interpolate
if t_des is None:
# Default value for t_des
t_des = np.linspace(0, self.cs.run_time, y_des.shape[0])
else:
# Warp time to start from zero and end up to T
t_des -= t_des[0]
t_des /= t_des[-1]
t_des *= self.cs.run_time
time = np.linspace(0, self.cs.run_time, self.cs.timesteps)
path_gen = scipy.interpolate.interp1d(t_des, y_des.transpose())
path = path_gen(time)
y_des = path.transpose() # timesteps x n_dmps
# calculate velocity of y_des with central differences
if self.dim == 'position':
if dy_des is None:
dy_des = np.zeros([self.n_dmps, self.cs.timesteps])
for i in range(self.n_dmps):
dy_des[i] = np.gradient(path[i]) / self.dt
# dy_des = np.gradient(y_des, axis=1) / self.dt
# calculate acceleration of y_des with central differences
if ddy_des is None:
ddy_des = np.zeros([self.n_dmps, self.cs.timesteps])
for i in range(self.n_dmps):
ddy_des[i] = np.gradient(dy_des[i]) / self.dt
dy_des = dy_des.transpose() # timesteps x n_dmps
ddy_des = ddy_des.transpose()
# find the force required to move along this trajectory
f_target = np.zeros([self.n_dmps, self.cs.timesteps])
if self.form == 'mod':
f_target = (
(ddy_des / self.K - (self.goal - y_des) +
self.D * dy_des / self.K).transpose() + np.reshape(
(self.goal - self.y0), [self.n_dmps, 1]) *
self.cs.rollout()) # n_dmps x timesteps
elif self.form == 'old':
# f_target = ((ddy_des - self.K * (self.goal - y_des) + self.D * dy_des)/(self.goal - self.y0)).transpose()
f_target = np.dot(np.linalg.inv(np.diag(self.goal - self.y0)),
(ddy_des - self.K * (self.goal - y_des) +
self.D * dy_des).transpose())
elif self.dim == 'orientation':
dq_des = np.zeros([self.n_dmps, self.cs.timesteps])
# # Ude, 2014, "Orientation in Cartesian Space Dynamic Movement Primitives"
angular_vel = np.zeros([self.timesteps, self.n_dmps - 1])
d_angular_vel = np.zeros([self.n_dmps - 1, self.timesteps])
for i in range(self.n_dmps):
dq_des[i] = np.gradient(
path[i]) / self.dt # dq, n_dmps x timesteps
for index, dq in enumerate(dq_des.T):
q = path.T[index]
angular_vel[index] = 2 * rm.quaternion_multiply(
dq, rm.quaternion_conjugate(q))[
1:] # w(t)=Im(2*dq(t)*q_conj(t))
for i in range(self.n_dmps - 1):
d_angular_vel[i] = np.gradient(angular_vel.T[i]) / self.dt
if dy_des is None:
dy_des = angular_vel
if ddy_des is None:
ddy_des = d_angular_vel
ddy_des = ddy_des.transpose() # d_eta, timesteps x n_dmps
import matplotlib.pyplot as plt
# fig, axes = plt.subplots(3, 2)
# axes[0, 0].plot(dy_des[:, 0], '--r', label='$\eta_x$'); axes[0, 0].legend()
# axes[1, 0].plot(dy_des[:, 1], '--g', label='$\eta_y$'); axes[1, 0].legend()
# axes[2, 0].plot(dy_des[:, 2], '--b', label='$\eta_z$'); axes[2, 0].legend()
# axes[0, 1].plot(ddy_des[:, 0], '-r', label='$\dot{\eta}_x$'); axes[0, 1].legend()
# axes[1, 1].plot(ddy_des[:, 1], '-g', label='$\dot{\eta}_y$'); axes[1, 1].legend()
# axes[2, 1].plot(ddy_des[:, 2], '-b', label='$\dot{\eta}_z$'); axes[2, 1].legend()
# fig.tight_layout()
f_target = np.zeros([self.cs.timesteps, self.n_dmps - 1])
eq_g_0 = q_oper.quaternion_error(self.goal, self.y0)
for t in range(self.cs.timesteps):
eq = q_oper.quaternion_error(self.goal, y_des[t])
f_target[t] = np.dot(
np.linalg.inv(np.diag(eq_g_0)),
ddy_des[t] - self.K * eq + self.D * dy_des[t])
# # Koutras, 2019, "A correct formulation for the orientation DMPs for robot control in the Cartesian space"
# if dy_des is None:
# dy_des = np.zeros([self.n_dmps-1, self.cs.timesteps])
#
# for index, dq in enumerate(dq_des.T):
# q = path.T[index]
# q_conj = rm.quaternion_conjugate(q)
# j_q = q_oper.calculate_jacobian(rm.quaternion_multiply(self.goal, q_conj))[1] # Jacobian_q matrix [3x4]
# d_eq = -2*np.dot(j_q, rm.quaternion_multiply(self.goal,
# rm.quaternion_multiply(q_conj, rm.quaternion_multiply(dq, q_conj))).transpose())
# dy_des[:, index] = d_eq.flatten()
# if ddy_des is None:
# ddy_des = np.zeros([self.n_dmps-1, self.cs.timesteps])
# for i in range(self.n_dmps-1):
# ddy_des[i] = np.gradient(dy_des[i]) / self.dt
# dy_des = dy_des.transpose() # timesteps x n_dmps
# ddy_des = ddy_des.transpose()
# f_target = np.zeros([self.n_dmps-1, self.cs.timesteps])
# eq_g_0 = q_oper.quaternion_error(self.goal, self.y0)
# f_target = f_target.T
# for t in range(self.cs.timesteps):
# eq = q_oper.quaternion_error(self.goal, y_des[t])
#
# f_target[t] = np.dot(np.diag(eq_g_0), ddy_des[t] - self.K*eq + self.D*dy_des[t])
f_target = f_target.T
# efficiently generate weights to realize f_target
self.gen_weights(f_target)
if plot is True:
# plot the basis function activations
import matplotlib.pyplot as plt
plt.figure()
plt.subplot(211)
psi_track = self.gen_psi(self.cs.rollout())
plt.plot(np.transpose(psi_track))
plt.title("basis functions")
# plot the desired forcing function vs approx
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
plt.plot(f_target[ii, :], "--", label="f_target %i" % ii)
for ii in range(self.n_dmps):
plt.subplot(2, self.n_dmps, self.n_dmps + 1 + ii)
plt.plot(
np.sum(np.transpose(psi_track) * self.w[ii], axis=1) *
self.dt,
label="w*psi %i" % ii,
)
plt.legend()
plt.title("DMP forcing function")
plt.tight_layout()
plt.show()
self.reset_state()
self.learned_position = self.goal - self.y0
return y_des
def rollout(self, timesteps=None, **kwargs):
"""Generate a system trial, no feedback is incorporated."""
self.reset_state()
if timesteps is None:
if "tau" in kwargs:
timesteps = int(self.timesteps / kwargs["tau"])
else:
timesteps = self.timesteps
# set up tracking vectors
if self.dim == 'orientation':
y_track = np.zeros((timesteps, self.n_dmps)) # quaternion
dy_track = np.zeros((timesteps, self.n_dmps - 1))
ddy_track = np.zeros((timesteps, self.n_dmps - 1))
elif self.dim == 'position':
y_track = np.zeros((timesteps, self.n_dmps))
dy_track = np.zeros((timesteps, self.n_dmps))
ddy_track = np.zeros((timesteps, self.n_dmps))
for t in range(timesteps):
# run and record timestep
y_track[t], dy_track[t], ddy_track[t] = self.step(**kwargs)
return y_track, dy_track, ddy_track
def reset_state(self):
"""Reset the system state"""
self.y = self.y0.copy()
if self.dim == 'position':
self.dy = np.zeros(self.n_dmps)
self.ddy = np.zeros(self.n_dmps)
elif self.dim == 'orientation':
self.dy = np.zeros(self.n_dmps - 1)
self.ddy = np.zeros(self.n_dmps - 1)
self.cs.reset_state()
def step(self, tau=1.0, error=0.0, external_force=None):
"""Run the DMP system for a single timestep.
tau float: scales the timestep
increase tau to make the system execute faster
error float: optional system feedback
"""
if self.rescale == 'rotodilatation':
M = roto_dilatation(self.learned_position, self.goal - self.y0)
elif self.rescale == 'diagonal':
M = np.diag((self.goal - self.y0) / self.learned_position)
else:
M = np.eye(self.n_dmps)
error_coupling = 1.0 / (1.0 + error)
# run canonical system
x = self.cs.step(tau=tau, error_coupling=error_coupling)
# generate basis function activation
psi = self.gen_psi(x)
if self.dim == 'position':
if self.form == 'mod':
# generate the forcing term
f = (np.dot(self.w, psi[:, 0])) / np.sum(psi) * x
f = np.nan_to_num(np.dot(M, f))
# DMP acceleration
self.ddy = (self.K * (self.goal - self.y) - self.D * self.dy -
self.K * (self.goal - self.y0) * x + self.K * f)
if external_force is not None:
self.ddy += external_force
self.dy += self.ddy * tau * self.dt * error_coupling
self.y += self.dy * tau * self.dt * error_coupling
else:
# generate the forcing term
f = self.gen_front_term(x, self.n_dmps) * (np.dot(
self.w, psi[:, 0])) / np.sum(psi)
# DMP acceleration
self.ddy = (self.K * (self.goal - self.y) - self.D * self.dy +
f)
if external_force is not None:
self.ddy += external_force
self.dy += self.ddy * tau * self.dt * error_coupling
self.y += self.dy * tau * self.dt * error_coupling
elif self.dim == 'orientation':
# generate the forcing term
f = self.gen_front_term(x, self.n_dmps - 1) * (np.dot(
self.w, psi[:, 0])) / np.sum(psi)
# # Ude, 2014, "Orientation in Cartesian Space Dynamic Movement Primitives"
self.ddy = (self.K * q_oper.quaternion_error(self.goal, self.y) -
self.D * self.dy + f)
if external_force is not None:
self.ddy += external_force
self.dy += self.ddy * tau * self.dt * error_coupling
# w_q = np.concatenate((np.array([0]), self.dy), axis=0) # w_q = [0, w]
# dq = (rm.quaternion_multiply(1 / 2 * w_q, self.y) * tau * error_coupling).flatten()
q_delta = (q_oper.vector_exp(1 / 2 * self.dy * tau *
error_coupling * self.dt)).flatten()
self.y = rm.quaternion_multiply(q_delta, self.y)
# Koutras, 2019, "A correct formulation for the orientation DMPs for robot control in the Cartesian space"
# # DMP acceleration
# eq = q_oper.quaternion_error(self.goal, self.y)
# self.ddy = (self.K * (0 - eq) - self.D * self.dy + f)
# # print('ddy =', self.ddy)
# if external_force is not None:
# self.ddy += external_force
# self.dy += self.ddy * tau * self.dt * error_coupling
# # print('dy =', self.dy)
# q_conj = rm.quaternion_conjugate(self.y)
# goal_conj = rm.quaternion_conjugate(self.goal)
# dq = -1/2 * self.y * goal_conj * \
# np.dot(q_oper.calculate_jacobian(rm.quaternion_multiply(self.goal, q_conj))[0], self.dy) * self.y
# self.y += dq * tau * self.dt * error_coupling
return self.y, self.dy, self.ddy
