#
# # b = np.array([ [-0.147], [0.0], [-0.2 ], [0.0] ])
# # x = coef(b, 10)
# # fit = poly.Polynomial(x.flatten())
# t = np.linspace(0,10,count)
# # 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, :])
#
# b = np.array([ [-0.147], [0.0], [-0.2 ], [0.0] ])
# x = coef(b, 10)
# fit = poly.Polynomial(x.flatten())
t = np.linspace(0, 10, count)
# 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, :])
# t = np.linspace(0,10,count)
# # 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)
#
# 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()
# jp1_list = jp_1 #+ gauss(0, .01)
# jp2_list = jp_2 #+ gauss(0, .01)
# jp3_list = jp_3 #+ gauss(0, .01)
# jp4_list = jp_4 #+ gauss(0, .01)
# jp5_list = jp_5 #+ gauss(0, .01)
# 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)
y = y_prime + gauss(-0.05, 0.05)
knee.append(y)
b = np.array([[0.257], [0.0], [0.0], [0.0]])
x = coef(b, 10)
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:
def train_model(file_name, bins=15, save=True):
angles = get_data()
traj = [angles["Lhip"], angles["Lknee"], angles["Lankle"], angles["Rhip"], angles["Rknee"], angles["Rankle"]]
trainer = TPGMMTrainer.TPGMMTrainer(demo=traj, file_name=file_name, n_rf=bins, dt=0.01, reg=[1e-3, 1e-3, 1e-3, 1e-3, 1e-3, 1e-3],
poly_degree=[20, 20, 20, 20, 20, 20])
return trainer.train(save)
y = y_prime + gauss(-0.01, 0.01)
knee.append(y)
b = np.array([[-0.147], [0.0], [-0.2], [0.0]])
x = coef(b, 10)
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:
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()
# 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-5, 1e-7, 1e-7],
poly_degree=[20, 20, 20])
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)
jp1_list = jp_1 + gauss(-0.01, 0.01)
jp2_list = jp_2 + gauss(-0.01, 0.01)
jp3_list = jp_3 + gauss(-0.01, 0.01)
jp4_list = jp_4 + gauss(-0.01, 0.01)
jp5_list = jp_5 + gauss(-0.01, 0.01)
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=[jp0, jp1, jp2, jp3, jp4, jp5],
file_name="real_test_6",
n_rf=10,
dt=0.01,
reg=[1e-1],
poly_degree=[15, 15, 15, 15, 15, 15])
trainer.train()
runner = TPGMMRunner.TPGMMRunner("real_test_6")
#
#
path = runner.run()
with open('result_test_5', 'w') as f:
json.dump(path.tolist(), f)
fig, axs = plt.subplots(2)
for p in jp0:
axs[0].plot(p)