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 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 __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
