goaly = -120.0
startx = 30.0
starty = 80.0
K = 1000.0
D = 40.0
xseq = np.linspace(startx, goalx, num=100)
yseq = np.linspace(starty, goaly, num=100)
step = 0
step_change = -1
thetas1, thetas2 = [], []
for i in range(len(xseq)):
theta1, theta2 = arm.inverse_kinematics(xseq[i], yseq[i])
thetas1.append(theta1)
thetas2.append(theta2)
demonstration, velocities, accelerations, times = diff_demonstration(thetas1, tau)
dmp1 = DMP(basis, K, D, demonstration[0], demonstration[-1])
#dmp1.learn_dmp(times, demonstration, velocities, accelerations)
demonstration, velocities, accelerations, times = diff_demonstration(thetas2, tau)
dmp2 = DMP(basis, K, D, demonstration[0], demonstration[-1])
#dmp2.learn_dmp(times, demonstration, velocities, accelerations)
dmps.append( dmp1 )
dmps.append( dmp2 )
