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()