dmp2 = DMP(basis, K, D, demonstration[0], demonstration[-1])
#dmp2.learn_dmp(times, demonstration, velocities, accelerations)
dmps.append( dmp1 )
dmps.append( dmp2 )
weights = list()
for i in range(dmp1.weights.shape[0]):
w = list( (dmp1.weights[i], dmp2.weights[i]) )
weights.extend(w)
result = fmin_bfgs(error_func, [ weights ])
x1, x1dot, x1ddot, t1 = dmp1.run_dmp(tau, 0.01, dmp1.start, dmp1.goal)
plt.plot(times, thetas1, "r")
plt.plot(t1, x1, "b")
plt.show()
x2, x2dot, x2ddot, t2 = dmp2.run_dmp(tau, 0.01, dmp2.start, dmp2.goal)
plt.plot(times, thetas2, "r")
plt.plot(t2, x2, "b")
plt.show()
theta1, theta2 = None, None
while True:
display.fill(white)
theta1 = round(x1[step], 3)
K = 50.0
D = 10.0
dmp1 = DMP(basis, K, D, 0.0, parameters[0])
dmp2 = DMP(basis, K, D, 0.0, parameters[1])
dmp3 = DMP(basis, K, D, 0.0, parameters[2])
count = 3
for i in range(basis):
dmp1.weights[i] = parameters[count]
dmp2.weights[i] = parameters[count+basis]
dmp3.weights[i] = parameters[count+(2*basis)]
count += 1
last_step = 250
xpos, xdot, xddot, times = dmp1.run_dmp(tau, dmp_dt, dmp1.start, dmp1.goal)
x1, x1dot, x1ddot, t1 = diff_demonstration(xpos[:last_step], tau)
dmp1new = DMP(basis, K, D, 0.0, xpos[last_step])
dmp1new.learn_dmp(t1, x1, x1dot, x1ddot)
xpos1, xdot1, xddot1, times1 = dmp1new.run_dmp(tau, dmp_dt, dmp1new.start, dmp1new.goal)
plt.plot(t1, x1, times1, xpos1, times, xpos)
plt.show()
xpos, xdot, xddot, times = dmp2.run_dmp(tau, dmp_dt, dmp2.start, dmp2.goal)
x2, x2dot, x2ddot, t2 = diff_demonstration(xpos[:last_step], tau)
dmp2new = DMP(basis, K, D, 0.0, xpos[last_step])
dmp2new.learn_dmp(t2, x2, x2dot, x2ddot)
xpos2, xdot2, xddot2, times2 = dmp2new.run_dmp(tau, dmp_dt, dmp2new.start, dmp2new.goal)
plt.plot(t2, x2, times2, xpos2, times, xpos)
plt.plot(t_demo, x_demo, c="r")
plt.show()
if __name__ == '__main__':
K = 1000.0
D = 40.0
basis = 50
#################################################################################
###################### Synthetic Example ########################################
t_demonstration = 10
demonstration, velocities, accelerations, times = generate_example(t_demonstration)
dmp = DMP(basis, K, D, demonstration[0], demonstration[-1])
dmp.learn_dmp(times, demonstration, velocities, accelerations)
tau = times[-1] - times[0]
x, xdot, xddot, t = dmp.run_dmp(tau, 0.01, demonstration[0], demonstration[-1])
plotDMP(times, demonstration, t, x)
reverse_pos = [pos[0] for pos in list(reversed(x))]
demonstration1, velocities1, accelerations1, times = diff_demonstration(reverse_pos, t_demonstration)
dmp1 = DMP(basis, K, D, demonstration1[0], demonstration1[-1])
print "DMP pos: ", dmp1.pos
print "Demo: ", demonstration1[0]
dmp1.learn_dmp(times, demonstration1, velocities1, accelerations1)
print "Demo: ", demonstration1[0]
tau = times[-1] - times[0]
x, xdot, xddot, t = dmp1.run_dmp(tau, 0.01, demonstration1[0], demonstration1[-1])
dmp1 = DMP(basis, K, D, world.arm.joint1.angle, parameters[0])
dmp2 = DMP(basis, K, D, world.arm.joint2.angle, parameters[1])
dmp3 = DMP(basis, K, D, world.arm.joint3.angle, parameters[2])
all_pos = list()
count = 3
for i in range(basis):
dmp1.weights[i] = parameters[count]
dmp2.weights[i] = parameters[count+basis]
dmp3.weights[i] = parameters[count+(2*basis)]
count += 1
x1, x1dot, x1ddot, t1 = dmp1.run_dmp(tau, dmp_dt, dmp1.start, dmp1.goal)
x2, x2dot, x2ddot, t2 = dmp2.run_dmp(tau, dmp_dt, dmp2.start, dmp2.goal)
x3, x3dot, x3ddot, t3 = dmp3.run_dmp(tau, dmp_dt, dmp3.start, dmp3.goal)
x1 = normalize_dmp_pos(x1)
x2 = normalize_dmp_pos(x2)
x3 = normalize_dmp_pos(x3, 0, math.pi)
plt.plot(t1, x1, "b")
plt.show()
plt.plot(t2, x2, "r")
plt.show()
plt.plot(t3, x3, "g")
plt.show()
