def dmpTrain(q, qd, qdd, dt, nSteps):
params = dmpParams()
# Set dynamic system parameters
params.alphaz = 3 / (nSteps * dt - dt)
params.alpha = 25
params.beta = 6.25
params.Ts = nSteps * dt - dt
params.tau = 1
params.nBasis = 50
params.goal = np.asarray(q[:, -1]) # np.asarray([0.3, -0.8])
# Daniel: This should actually be Psi, right?
Phi = getDMPBasis(params, dt, nSteps)
# Compute the forcing function
ft = 1 / params.tau ** 2 * qdd - 1 / params.tau * (0 - qd) \
- params.alpha * params.beta * (params.goal.reshape(2, 1) - q) #
# Learn the weights
sigma = 10**-12
params.w = np.linalg.inv(Phi.transpose().dot(Phi) + sigma ** 2 * np.identity(Phi.shape[1])).dot(Phi.transpose()) \
.dot(ft.transpose())
params = dmpParams()
# Set dynamic system parameters
params.alphaz = 3 / (nSteps * dt)
params.alpha = 25
params.beta = 6.25
params.Ts = nSteps * dt
params.tau = 1
params.nBasis = 50
params.goal = q[:, -1]
Phi = getDMPBasis(params, dt, nSteps)
# Compute the forcing function
a = qdd / params.tau**2
b = params.alpha * params.beta * (params.goal[:, np.newaxis] - q)
c = params.alpha * qd / params.tau
ft = a - b + c
# Learn the weights
sigma = 10**-12
N, M = Phi.shape
pseudo_inv = np.linalg.inv(Phi.T @ Phi + sigma**2 * np.eye(M, M))
params.w = pseudo_inv @ Phi.T @ ft.T
return params
def dmpTrain(q, qd, qdd, dt, nSteps):
params = dmpParams()
#Set dynamic system parameters
T = dt * nSteps - dt
params.alphaz = 3 / T
params.alpha = 25
params.beta = 6.25
params.Ts = T
params.tau = 1
params.nBasis = 50
params.goal = np.array([[0.3, -0.8]])
sigma = 1e-8
Phi = getDMPBasis(params, dt, nSteps)
print("Phi: ", Phi)
#Compute the forcing function
ft = qdd / (params.tau**
2) - params.alpha * (params.beta *
(params.goal.T - q) - qd / params.tau)
#Learn the weights
sigma = 1e-8
pseudo_inv = np.linalg.inv(
(np.dot(Phi.T, Phi) + sigma * np.eye(Phi.shape[1])))
params.w = np.dot(np.dot(pseudo_inv, Phi.T), ft.T)
return params
def dmpTrain(q, qd, qdd, dt, nSteps):
# shape of q, qd, qdd is (2, 1499)
# nSteps = 1499
params = dmpParams()
#Set dynamic system parameters
params.alphaz = 3 / (nSteps * dt)
params.alpha = 25
params.beta = 6.25
# params.Ts = nSteps * dt
params.tau = 1
params.nBasis = 50
# Phi is the basis functions phi(z)
Phi = getDMPBasis(params, dt, nSteps)
# Parameters need to compute forcing function.
a = params.alpha
b = params.beta
t = params.tau
g = params.goal
# Compute the forcing function
ft = np.zeros(shape=(1499, 2))
for i in range(nSteps):
ft[i, :] = qdd[:, i] / np.square(t) - a * (b * (g - q[:, i]) -
qd[:, i] / t)
#Learn the weights
params.w = np.matmul(np.linalg.pinv(Phi), ft)
return params
def simSys(states, dmpParams, dt, nSteps):
Phi = getDMPBasis(dmpParams, dt, nSteps)
for i in range(nSteps - 1):
qdd = dmpCtl(dmpParams, Phi[i, :].transpose(), states[i, ::2],
states[i, 1::2].transpose())
# raw_input()
states[i + 1, 1::2] = states[i, 1::2] + dt * qdd
states[i + 1, ::2] = states[i, ::2] + dt * states[i, 1::2]
return states
def simSys(states, dmpParams, dt, nSteps):
Phi = getDMPBasis(dmpParams, dt, nSteps)
for i in range(nSteps - 1):
# TO-DO: implement the dmpCtl() function, to get the y_dd(same symbol as the exercise)
# y_dd = t^2( a(b(g-y)-y_d/t) + fw(z))
qdd = dmpCtl(dmpParams, Phi[i, :].transpose(), states[i, ::2],
states[i, 1::2].transpose())
#raw_input()
states[i + 1, 1::2] = states[i, 1::2] + dt * qdd
states[i + 1, ::2] = states[i, ::2] + dt * states[i, 1::2]
return states
def dmpTrain (q, qd, qdd, dt, nSteps):
params = dmpParams()
#Set dynamic system parameters
params.alphaz =
params.alpha =
params.beta =
params.Ts =
params.tau =
params.nBasis =
params.goal =
Phi = getDMPBasis(params, dt, nSteps)
#Compute the forcing function
ft =
#Learn the weights
params.w =
return params