def calculate_imitation_metric(file_name): angles = get_data() demos = [angles["Lhip"], angles["Lknee"], angles["Lankle"]] runner = TPGMMRunner.TPGMMRunner(file_name) path = runner.run() print(path[:,0]) alpha = 1.0 manhattan_distance = lambda x, y: abs(x - y) costs = [] for i in range(3): imitation = path[:, i] T = len(imitation) M = len(demos[i]) metric = 0.0 t = [] t.append(1.0) for k in range(1, T): t.append(t[k - 1] - alpha * t[k - 1] * 0.01) # Update of decay term (ds/dt=-alpha s) ) t = np.array(t) for m in range(M): d, cost_matrix, acc_cost_matrix, path_im = dtw(imitation, demos[i][m], dist=manhattan_distance) data_warp = [demos[i][m][path_im[1]][:imitation.shape[0]]] coefs = poly.polyfit(t, data_warp[0], 20) ffit = poly.Polynomial(coefs) y_fit = ffit(t) metric += np.sum(abs(y_fit - imitation.flatten())) costs.append(metric / (M * T)) print("cost ") print(costs) return costs
def lin(): hip_traj = [] path = [] runner = TPGMMRunner.TPGMMRunner("/home/nathaniel/catkin_ws/src/ambf_walker/config/gotozero.pickle") count = 0 x = runner.get_start() v0 = np.zeros(len(x)).reshape((-1, 1)) y = np.concatenate((x, v0)) while count < runner.get_length(): runner.step() u = runner.ddx x = runner.x dx = runner.dx # u = Kp.dot(x - y[0:6]) + Kd.dot(dx - y[6:]) tau = np.asarray([0.0] * 6) rbdl.InverseDynamics(my_model, x.flatten(), dx.flatten(), u.flatten(), u.flatten()) du = np.asarray([.1] * 6) traj = np.concatenate((x, dx)) A, b = model.finite_differences(my_model, traj.flatten(), tau, h=0.01) y = model.runge_integrator_lin(A, b, y.flatten(), 0.01, du) hip_traj.append(y[0]+x[0]) count += 1 path.append(x[0]) return hip_traj, path
def __init__(self, model, runner): """ :param model: :param kp: :param kd: """ super(MPCController, self).__init__(model) self.runner = TPGMMRunner.TPGMMRunner("/home/nathanielgoldfarb/catkin_ws/src/ambf_walker/Train/gotozero.pickle") self.pub = rospy.Publisher("MPC_points", Float32MultiArray, queue_size=1,latch=True) self.x = []
def __init__(self, model, runner): """ :param model: :param kp: :param kd: """ super(LQRController, self).__init__(model) self.runner = TPGMMRunner.TPGMMRunner("/home/jack/catkin_ws/src/ambf_walker/Train/gotozero.pickle") self.x = [] self.setup()
def gen_traj(file_name=None): angles = get_data() # traj = [angles["Lhip"], angles["Lknee"], angles["Lankle"], angles["Rhip"], angles["Rknee"], angles["Rankle"]] min_length = 5000000 for arr in angles["Rhip"]: min_length = min(min_length, len(arr)) matplotlib.rcParams.update({'font.size': 25}) fig, ax = plt.subplots(3) fig.tight_layout(pad=1.0, h_pad=0.15, w_pad=None, rect=None) ax[0].set_title("Left Hip") ax[1].set_title("Left Knee") ax[2].set_title("Left Ankle") # ax[0, 1].set_title("Right Hip") # ax[1, 1].set_title("Right Knee") # ax[2, 1].set_title("Right Ankle") ax[2].set_xlabel("Gait %") #ax[2, 1].set_xlabel("Gait %") ax[0].set_ylabel("Angle(deg)") ax[1].set_ylabel("Angle(deg)") ax[2].set_ylabel("Angle(deg)") for i in range(len(angles["Rhip"])): ax[0].plot(np.linspace(0, 100, len(angles["Lhip"][i])), np.rad2deg(angles["Lhip"][i])) ax[1].plot(np.linspace(0, 100, len(angles["Lknee"][i])), np.rad2deg(angles["Lknee"][i])) ax[2].plot(np.linspace(0, 100, len(angles["Lankle"][i])), np.rad2deg( angles["Lankle"][i])) # ax[0, 1].plot(np.linspace(0, 100, len(angles["Rhip"][i])), np.rad2deg(angles["Rhip"][i])) # ax[1, 1].plot(np.linspace(0, 100, len(angles["Rknee"][i])), np.rad2deg(angles["Rknee"][i])) # ax[2, 1].plot(np.linspace(0, 100, len(angles["Rankle"][i])), np.rad2deg(angles["Rankle"][i])) if file_name: runner = TPGMMRunner.TPGMMRunner(file_name + ".pickle") path = np.array(runner.run()) ax[0].plot( np.linspace(0, 100, len(path[:, 0] )), np.rad2deg(path[:, 0]), linewidth=4) ax[1].plot( np.linspace(0, 100, len(path[:, 1] )), np.rad2deg(path[:, 1]), linewidth=4) ax[2].plot( np.linspace(0, 100, len(path[:, 2] )), np.rad2deg(path[:, 2]), linewidth=4) # ax[0, 1].plot( np.linspace(0, 100, len(path[:, 3] )), np.rad2deg(path[:, 3]), linewidth=2) # ax[1, 1].plot( np.linspace(0, 100, len(path[:, 4] )), np.rad2deg(path[:, 4]), linewidth=2) # ax[2, 1].plot( np.linspace(0, 100, len(path[:, 5] )), np.rad2deg(path[:, 5]), linewidth=2) # plt.show()
# angles = plot_joint_angles(["/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_00/subject_00 walk_00.csv", # "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_03/subject_03_walk_00.csv", # "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_04/subject_04_walk_00.csv", # "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_05/subject_05_walk_00.csv", # "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_08/subject_08_walking_01.csv", # "/home/nathaniel/AIM_GaitData/Gaiting_stairs/subject_10/subject_10 walk_02.csv", # ], # [0, 0, 0, 0, 0, 2], # ["R", "R", "R", "R", "R", "R", "R", "R", "R", "R"], # ["Subject00", "Subject01", "Subject02", 'Subject03', "subject04", "Subject05", "Subject06", # "Subject07", "Subject08", "Subject10"]) traj = [angles["hip"],angles["knee"], angles['ankle']] trainer = TPGMMTrainer.TPGMMTrainer(demo=traj, file_name="leg", n_rf=15, dt=0.01, reg=[1e-8, 1e-8, 1e-8], poly_degree=[15, 15, 15]) trainer.train() runner = TPGMMRunner.TPGMMRunner("leg.pickle") path = np.array(runner.run()) fig, (ax1, ax2, ax3) = plt.subplots(3, 1) for p in angles["hip"]: ax1.plot(p) for p in angles["knee"]: ax2.plot(p) for p in angles["ankle"]: ax3.plot(p) ax1.plot(path[:, 0], linewidth=4) ax2.plot(path[:, 1], linewidth=4) ax3.plot(path[:, 2], linewidth=4) plt.show()
# for i in range(5): # z = z_prime + gauss(-.1, .1) # z_data.append(z) # # x_coordinates = path[:,0] # y_coordinates = path[:,1] # z_coordinates = path[:,2] trainer = TPGMMTrainer.TPGMMTrainer(demo=[jp0, jp1, jp2, jp3, jp4, jp5], file_name="test_2", n_rf=3, dt=0.05, reg=[2e-3], poly_degree=[15, 15, 15, 15, 15, 15]) trainer.train() runner = TPGMMRunner.TPGMMRunner("test_2") path = runner.run() with open('result_test_2', 'w') as f: json.dump(path.tolist(), f) # x_data_arr = np.array(x_data) # y_data_arr = np.array(y_data) # z_data_arr = np.array(z_data) # # print(x_data_arr[0, :]) # # fig = plt.figure() # ax = fig.add_subplot(111, projection='3d') # ax.scatter3D(x_data_arr[0, :], y_data_arr[0, :], z_data_arr[0, :], c='yellow',label='given trajectory 1') # ax.scatter3D(x_data_arr[1, :], y_data_arr[1, :], z_data_arr[1, :], c='blue',label='given trajectory 2') # ax.scatter3D(x_data_arr[2, :], y_data_arr[2, :], z_data_arr[2, :], c='green',label='given trajectory 3')
from GaitAnaylsisToolkit.LearningTools.Runner import TPGMMRunner import matplotlib.pyplot as plt runner = TPGMMRunner.TPGMMRunner( "/home/nathanielgoldfarb/catkin_ws/src/ambf_walker/config/walk2.pickle") joints = [] count = 0 while count < runner.get_length(): count += 1 runner.step() x = runner.x joints.append(x[0]) print(joints) plt.plot(joints) plt.show()
# z_prime = t z_prime = t + t**1.2 + np.exp(-t) #Z Coordinate z_data = [] for i in range(5): z = z_prime + gauss(-.1, .1) z_data.append(z) trainer = TPGMMTrainer.TPGMMTrainer(demo=[x_data, y_data, z_data], file_name="spiraltest", n_rf=3, dt=0.05, reg=[1e-3], poly_degree=[3, 3, 3]) trainer.train() runner = TPGMMRunner.TPGMMRunner("spiraltest") path = runner.run() x_coordinates = path[:, 0] y_coordinates = path[:, 1] z_coordinates = path[:, 2] x_data_arr = np.array(x_data) y_data_arr = np.array(y_data) z_data_arr = np.array(z_data) # print(x_data_arr[0, :]) fig = plt.figure() ax = fig.add_subplot(111, projection='3d') ax.scatter3D(x_data_arr[0, :], y_data_arr[0, :],
for h in hill: hip.append(-(np.pi / 180) * np.array(joints.hip.angle.x[h[0]])) knee.append(-(np.pi / 180) * np.array(joints.knee.angle.x[h[0]])) ankle.append(-(np.pi / 180) * np.array(joints.ankle.angle.x[h[0]])) hips.append(np.array(hip)) knees.append(np.array(knee)) ankles.append(np.array(ankle)) return hips, knees, ankles hills = get_index(file_list) hip, knee, ankle = make_traj(file_list, hills) for p in hip: plt.plot(p) trainer = TPGMMTrainer.TPGMMTrainer( [hip], "/home/nathaniel/catkin_ws/src/ambf_walker/config/hip2", 25, 0.01) trainer.train() runner = TPGMMRunner.TPGMMRunner( "/home/nathaniel/catkin_ws/src/ambf_walker/config/hip2.pickle") ddx = [] for i in range(runner.get_length()): x = runner.step() xx = runner.ddx[0] ddx.append(xx) plt.plot(ddx, linewidth=4) plt.show()
def f2(x, u, i): y = model.runge_integrator(my_model, x, 0.01, u) return np.array(y) ############################################## MODIFY #################### ###### original : return np.array(y) dynamics = FiniteDiffDynamics(f, 12, 6) file_name = "/home/jack/backup/leg1.pickle" #file_name = "/home/jack/catkin_ws/src/ambf_walker/Train/gotozero.pickle" runner = TPGMMRunner.TPGMMRunner(file_name) x_path = [] u_path = [] #us_init = np.random.uniform(-1, 1, (N-1, dynamics.action_size)) count = 0 N = runner.get_length() # print(N) while count < runner.get_length(): count += 1 runner.step() u_path.append(runner.ddx.flatten().tolist()) ##### set it to be random no from -5 to 5 (u_p) x = runner.x.flatten().tolist() + runner.dx.flatten().tolist() x_path.append(x)
# us = diff * np.tanh(u) + mean y = model.runge_integrator(my_model, x, 0.01, us) return np.array(y) def f2(x, u, i): y = model.runge_integrator(my_model, x, 0.01, u) return np.array(y) dynamics = FiniteDiffDynamics(f, 12, 6) runner = TPGMMRunner.TPGMMRunner("/home/nathanielgoldfarb/catkin_ws/src/ambf_walker/Train/gotozero.pickle") x_path = [] u_path = [] count = 0 N = runner.get_length() while count < runner.get_length(): count += 1 runner.step() u_path.append(runner.ddx.flatten().tolist()) x = runner.x.flatten().tolist() + runner.dx.flatten().tolist() x_path.append(x) u_path = u_path[:-1] expSigma = runner.get_expSigma()
# jp0.append(jp0_list) # jp1.append(jp1_list) # jp2.append(jp2_list) # jp3.append(jp3_list) # jp4.append(jp4_list) # jp5.append(jp5_list) # # trainer = TPGMMTrainer.TPGMMTrainer(demo=[jp5], file_name="try1_joint_5", n_rf=10, dt=0.01, reg=[1e-1], poly_degree=[15]) trainer.train() runner = TPGMMRunner.TPGMMRunner("try1_joint_5") # # path = runner.run() with open('result1_try_5', 'w') as f: json.dump(path.tolist(), f) fig, axs = plt.subplots(2) for p in jp5: axs[0].plot(p) axs[0].plot(path[:, 0], linewidth=4, color='black') # for p in jp1: # axs[1].plot(p)
fit = poly.Polynomial(x.flatten()) t = np.linspace(0, 10, count) y_prime = fit(t) ankle = [] for i in range(5): y = y_prime + gauss(-0.01, 0.01) ankle.append(y) trainer = TPGMMTrainer.TPGMMTrainer(demo=[hip, knee, ankle, hip, knee, ankle], file_name="simpletest", n_rf=6, dt=0.01, reg=[1e-3], poly_degree=[3, 3, 3, 3, 3, 3]) trainer.train() runner = TPGMMRunner.TPGMMRunner("simpletest") path = runner.run() fig, axs = plt.subplots(3) print(path) for p in hip: axs[0].plot(p) axs[0].plot(path[:, 0], linewidth=4, color='black') for p in knee: axs[1].plot(p) axs[1].plot(path[:, 1], linewidth=4, color='black') for p in ankle:
# # z_prime = t # z_prime = t + t**1.2 + np.exp(-t) # #Z Coordinate # z_data = [] # for i in range(5): # z = z_prime + gauss(-.1, .1) # z_data.append(z) trainer = TPGMMTrainer.TPGMMTrainer(demo=[jp0, jp1, jp2, jp3, jp4, jp5], file_name="real_test_1", n_rf=6, dt=0.05, reg=[1e-3], poly_degree=[3, 3, 3, 3, 3, 3]) trainer.train() runner = TPGMMRunner.TPGMMRunner("real_test_1") path = runner.run() # x_coordinates = path[:,0] # y_coordinates = path[:,1] # z_coordinates = path[:,2] # # x_data_arr = np.array(x_data) # y_data_arr = np.array(y_data) # z_data_arr = np.array(z_data) # # print(x_data_arr[0, :]) # # fig = plt.figure() # ax = fig.add_subplot(111, projection='3d') # ax.scatter3D(x_data_arr[0, :], y_data_arr[0, :], z_data_arr[0, :], c='yellow',label='given trajectory 1') # ax.scatter3D(x_data_arr[1, :], y_data_arr[1, :], z_data_arr[1, :], c='blue',label='given trajectory 2')
def get_runner(self): return TPGMMRunner.TPGMMRunner( "/home/nathaniel/catkin_ws/src/ambf_walker/config/gotozero.pickle")
fit = poly.Polynomial(x.flatten()) t = np.linspace(0, 10, 100) y_prime = fit(t) ankle = [] for i in range(10): y = y_prime + gauss(-0.05, 0.05) ankle.append(y) trainer = TPGMMTrainer.TPGMMTrainer(demo=[hip, knee, ankle, hip, knee, ankle], file_name="gotozero", n_rf=5, dt=0.01, reg=[1e-4], poly_degree=[3, 3, 3, 3, 3, 3]) trainer.train() runner = TPGMMRunner.TPGMMRunner("gotozero") path = runner.run() fig, axs = plt.subplots(3) print(path) for p in hip: axs[0].plot(p) axs[0].plot(path[:, 0], linewidth=4) for p in knee: axs[1].plot(p) axs[1].plot(path[:, 1], linewidth=4) for p in ankle:
def get_walker(self): project_root = dirname(dirname(__file__)) config_path = join(project_root, 'config/walk2.pickle') return TPGMMRunner.TPGMMRunner(config_path)
t = np.linspace(-10, 10, 100) y_prime = fit(t) knee = [] for i in range(4): y = y_prime + gauss(-2, 2) #y = np.append(y, np.flip(y)) knee.append(y) trajs = [hip] trainer = TPGMMTrainer.TPGMMTrainer(demo=trajs, file_name="poly9", n_rf=15, dt=0.01, reg=1e-5, poly_degree=[5, 5]) trainer.train() runner = TPGMMRunner.TPGMMRunner("poly9.pickle") path = np.array(runner.run()) fig, (ax1, ax2) = plt.subplots(1, 2) for p in hip: ax1.plot(p) for p in knee: ax2.plot(p) ax1.plot(path[:, 0], linewidth=4) #ax2.plot(path[:, 1], linewidth=4) plt.show()