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