class MuellingDmp:
'''
Muelling dmp without amplitude scaling (eta)
'''
def __init__(self, executionTime, startPos, startVel, startAcc, goalPos, goalVel, cs, numWeights, overlap, use_vel_scaling):
self.T = executionTime
self.cs = cs
self.alpha = 25.0
self.beta = 6.25
self.g = goalPos
self.gd = goalVel
self.gdd = 0.0 #has to be 0
self.y = startPos
self.y0 = startPos
self.yd0 = startVel
self.ydd0 = startAcc
self.ydd = startAcc
self.v = self.T * self.yd0
self.startV = self.v
self.fop = Fop(0.0, startPos, startVel, startAcc, executionTime, goalPos, goalVel, self.gdd)
self.rbf = Rbf(cs, executionTime, numWeights, overlap)
self.amplitude = goalPos - startPos
self.amplitude2 = goalVel - startVel
self.use_vel_scaling = use_vel_scaling
def step(self, dt):
z = self.cs.step(dt)
t = self.cs.t
f = self.rbf.evaluate(z)
# eta = (self.g - self.y0) / self.amplitude
#eta = exp((self.g - self.y0) - self.amplitude)
eta = (self.gd - self.yd0) / self.amplitude2
if self.use_vel_scaling:
f *= eta
g, gd, gdd = self.fop.eval(t) #moving target
vd = ((self.alpha * (self.beta * (g - self.y) + gd * self.T - self.v) + gdd * self.T**2 + f) / self.T) * dt
yd = self.v / self.T * dt
self.y += yd
self.v += vd
self.ydd = vd / self.T
def imitate(self, times, positions, velocities, accelerations, dt):
'''
first position at t=0 and last position at t = executionTime
dt = sampling dt
'''
fop = Fop(0.0, positions[0], velocities[0], accelerations[0], times[-1], positions[-1], velocities[-1], accelerations[-1])
g, gd, gdd = fop.evalArray(times)
references = self.T**2 * accelerations - self.alpha * ( self.beta * (g - positions) + self.T * gd - self.T * velocities) - self.T**2 * gdd
phases = self.cs.get_phases(times)
weights = np.ndarray(self.rbf.numWeights)
for i in range(self.rbf.numWeights):
psi = self.rbf.psi(i, phases)
psiD = np.diag(psi)
weights[i] = np.linalg.inv([[np.dot(phases.T, np.dot(psiD, phases))]]) * np.dot(phases.T, np.dot(psiD, references))
self.rbf.set_weights(weights)
self.amplitude = positions[-1] - positions[0]
def reset(self, cs, g, T, y0, gd):
self.cs = cs
self.g = g
self.gd = gd
self.T = T
self.y = y0
self.v = self.startV
self.y0 = y0
self.fop = Fop(0.0, self.y0, self.yd0, self.ydd0, self.T, self.g, self.gd, self.gdd)
def run(self, dt):
'''
runs the whole dmp and returns ([ts], [ys], [yds])
'''
ts = []
ys = []
yds = []
ydds = []
t = 0.0
while t < self.T:
ts.append(t)
ys.append(self.y)
yds.append(self.v / self.T)
ydds.append(self.ydd)
t += dt
self.step(dt)
ts.append(t)
ys.append(self.y)
yds.append(self.v / self.T)
ydds.append(self.ydd)
return (ts, np.asarray(ys), np.asarray(yds), np.asarray(ydds))
class DmpWithImitation:
'''
A simple Ijspeert dmp with forcing term
'''
def __init__(self, executionTime, startPos, startVel, goalPos, cs,
numWeights, overlap, use_scaling):
self.T = executionTime
self.cs = cs
self.alpha = 25.0
self.beta = 6.25
self.g = goalPos
self.y = startPos
self.startPos = startPos
self.z = self.T * startVel
self.startZ = self.z
self.rbf = Rbf(cs, executionTime, numWeights, overlap)
self.amplitude = 0
self.use_scaling = use_scaling
def step(self, dt):
z = self.cs.step(dt)
f = self.rbf.evaluate(z)
if self.use_scaling:
f *= (self.g - self.startPos) / self.amplitude
zd = ((self.alpha * (self.beta *
(self.g - self.y) - self.z) + f) / self.T) * dt
yd = self.z / self.T * dt
self.y += yd
self.z += zd
def imitate(self, times, positions, dt):
'''
first position at t=0 and last position at t = executionTime
dt = sampling dt
'''
self.amplitude = positions[-1] - positions[0]
velocities = np.gradient(positions, dt)
accelerations = np.gradient(velocities, dt)
goal = positions[len(positions) - 1]
references = self.T**2 * accelerations - self.alpha * (
self.beta * (goal - positions) - self.T * velocities)
phases = self.cs.get_phases(times)
weights = np.ndarray(self.rbf.numWeights)
for i in range(self.rbf.numWeights):
psi = self.rbf.psi(i, phases)
psiD = np.diag(psi)
weights[i] = np.linalg.inv([[
np.dot(phases.T, np.dot(psiD, phases))
]]) * np.dot(phases.T, np.dot(psiD, references))
self.rbf.set_weights(weights)
def run(self, dt, startTime=0.0, endTime=None):
'''
runs the whole dmp and returns ([ts], [ys], [yds])
'''
ts = []
ys = []
yds = []
t = startTime
if endTime is None:
endTime = self.T
while t < endTime:
ts.append(t)
ys.append(self.y)
yds.append(self.z / self.T)
t += dt
self.step(dt)
#ts.append(t)
#ys.append(self.y)
#yds.append(self.z / self.T)
return (ts, ys, yds)
def reset(self, cs, goal, executionTime, start):
self.cs = cs
self.g = goal
self.T = executionTime
self.y = start
self.z = self.startZ
class MuellingDmp:
'''
Muelling dmp without amplitude scaling (eta)
'''
def __init__(self, executionTime, startPos, startVel, startAcc, goalPos,
goalVel, cs, numWeights, overlap, use_vel_scaling):
self.T = executionTime
self.cs = cs
self.alpha = 25.0
self.beta = 6.25
self.g = goalPos
self.gd = goalVel
self.gdd = 0.0 #has to be 0
self.y = startPos
self.y0 = startPos
self.yd0 = startVel
self.ydd0 = startAcc
self.ydd = startAcc
self.v = self.T * self.yd0
self.startV = self.v
self.fop = Fop(0.0, startPos, startVel, startAcc, executionTime,
goalPos, goalVel, self.gdd)
self.rbf = Rbf(cs, executionTime, numWeights, overlap)
self.amplitude = goalPos - startPos
self.amplitude2 = goalVel - startVel
self.use_vel_scaling = use_vel_scaling
def step(self, dt):
z = self.cs.step(dt)
t = self.cs.t
f = self.rbf.evaluate(z)
# eta = (self.g - self.y0) / self.amplitude
#eta = exp((self.g - self.y0) - self.amplitude)
eta = (self.gd - self.yd0) / self.amplitude2
if self.use_vel_scaling:
f *= eta
g, gd, gdd = self.fop.eval(t) #moving target
vd = ((self.alpha *
(self.beta *
(g - self.y) + gd * self.T - self.v) + gdd * self.T**2 + f) /
self.T) * dt
yd = self.v / self.T * dt
self.y += yd
self.v += vd
self.ydd = vd / self.T
def imitate(self, times, positions, velocities, accelerations, dt):
'''
first position at t=0 and last position at t = executionTime
dt = sampling dt
'''
fop = Fop(0.0, positions[0], velocities[0], accelerations[0],
times[-1], positions[-1], velocities[-1], accelerations[-1])
g, gd, gdd = fop.evalArray(times)
references = self.T**2 * accelerations - self.alpha * (
self.beta * (g - positions) + self.T * gd -
self.T * velocities) - self.T**2 * gdd
phases = self.cs.get_phases(times)
weights = np.ndarray(self.rbf.numWeights)
for i in range(self.rbf.numWeights):
psi = self.rbf.psi(i, phases)
psiD = np.diag(psi)
weights[i] = np.linalg.inv([[
np.dot(phases.T, np.dot(psiD, phases))
]]) * np.dot(phases.T, np.dot(psiD, references))
self.rbf.set_weights(weights)
self.amplitude = positions[-1] - positions[0]
def reset(self, cs, g, T, y0, gd):
self.cs = cs
self.g = g
self.gd = gd
self.T = T
self.y = y0
self.v = self.startV
self.y0 = y0
self.fop = Fop(0.0, self.y0, self.yd0, self.ydd0, self.T, self.g,
self.gd, self.gdd)
def run(self, dt):
'''
runs the whole dmp and returns ([ts], [ys], [yds])
'''
ts = []
ys = []
yds = []
ydds = []
t = 0.0
while t < self.T:
ts.append(t)
ys.append(self.y)
yds.append(self.v / self.T)
ydds.append(self.ydd)
t += dt
self.step(dt)
ts.append(t)
ys.append(self.y)
yds.append(self.v / self.T)
ydds.append(self.ydd)
return (ts, np.asarray(ys), np.asarray(yds), np.asarray(ydds))
class DmpWithImitation:
'''
A simple Ijspeert dmp with forcing term
'''
def __init__(self, executionTime, startPos, startVel, goalPos, cs, numWeights, overlap, use_scaling):
self.T = executionTime
self.cs = cs
self.alpha = 25.0
self.beta = 6.25
self.g = goalPos
self.y = startPos
self.startPos = startPos
self.z = self.T * startVel;
self.startZ = self.z
self.rbf = Rbf(cs, executionTime, numWeights, overlap)
self.amplitude = 0
self.use_scaling = use_scaling
def step(self, dt):
z = self.cs.step(dt)
f = self.rbf.evaluate(z)
if self.use_scaling:
f *= (self.g - self.startPos) / self.amplitude
zd = ((self.alpha * (self.beta * (self.g - self.y)- self.z) + f) / self.T) * dt
yd = self.z / self.T * dt
self.y += yd
self.z += zd
def imitate(self, times, positions, dt):
'''
first position at t=0 and last position at t = executionTime
dt = sampling dt
'''
self.amplitude = positions[-1] - positions[0]
velocities = np.gradient(positions, dt)
accelerations = np.gradient(velocities, dt)
goal = positions[len(positions) - 1]
references = self.T**2 * accelerations - self.alpha * (self.beta * (goal - positions) - self.T * velocities)
phases = self.cs.get_phases(times)
weights = np.ndarray(self.rbf.numWeights)
for i in range(self.rbf.numWeights):
psi = self.rbf.psi(i, phases)
psiD = np.diag(psi)
weights[i] = np.linalg.inv([[np.dot(phases.T, np.dot(psiD, phases))]]) * np.dot(phases.T, np.dot(psiD, references))
self.rbf.set_weights(weights)
def run(self, dt, startTime = 0.0, endTime = None):
'''
runs the whole dmp and returns ([ts], [ys], [yds])
'''
ts = []
ys = []
yds = []
t = startTime
if endTime is None:
endTime = self.T
while t < endTime:
ts.append(t)
ys.append(self.y)
yds.append(self.z / self.T)
t += dt
self.step(dt)
#ts.append(t)
#ys.append(self.y)
#yds.append(self.z / self.T)
return (ts, ys, yds)
def reset(self, cs, goal, executionTime, start):
self.cs = cs
self.g = goal
self.T = executionTime
self.y = start
self.z = self.startZ