def plotDMP(t_demo, x_demo, t_dmp, x_dmp): plt.plot(t_dmp, x_dmp, c="b") 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]
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.show()