Commit bd2fc238 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Improve ex 4

parent 62b033b6
......@@ -3,6 +3,7 @@ import LMPC_walking.second_order.plot_utils as plot_utils
from LMPC_walking.second_order.LIPM_to_whole_body import compute_foot_traj, interpolate_lipm_traj
import matplotlib.pyplot as plt
import ex_4_conf as conf
#import ex_4_long_conf as conf
# READ COM-COP TRAJECTORIES COMPUTED WITH LIPM MODEL
......@@ -86,3 +87,5 @@ plot_utils.plot_xy(time_ctrl, N_ctrl, foot_length, foot_width,
com[0,:].reshape((N_ctrl+1,1)),
com[1,:].reshape((N_ctrl+1,1)))
plt.plot( com_state_x[:,0], com_state_y[:,0], 'r* ', markersize=15,)
plt.gca().set_xlim([-0.2,0.4])
plt.gca().set_ylim([-0.3,0.3])
......@@ -26,6 +26,7 @@ import LMPC_walking.second_order.cost_function as cost_function
import LMPC_walking.second_order.constraints as constraints
import LMPC_walking.second_order.plot_utils as plot_utils
import ex_4_conf as conf
#import ex_4_long_conf as conf
# Inverted pendulum parameters:
# ----------------------------
......@@ -108,6 +109,9 @@ plot_utils.plot_y(time, N, min_admissible_CoP,
# -----------------------------------
plot_utils.plot_xy(time, N, foot_length, foot_width,
cop_ref, cop_x, cop_y, com_state_x, com_state_y)
import matplotlib.pyplot as plt
plt.gca().set_xlim([cop_ref[0,0]-0.2, cop_ref[-1,0]+0.2])
plt.gca().set_ylim([cop_ref[0,1]-0.2, cop_ref[-1,1]+0.2])
com_state_x = np.vstack((x_0, com_state_x))
com_state_y = np.vstack((y_0, com_state_y))
......
......@@ -6,6 +6,7 @@ import matplotlib.pyplot as plt
import plot_utils as plut
import time
import ex_4_conf as conf
#import ex_4_long_conf as conf
from tsid_biped import TsidBiped
print "".center(conf.LINE_WIDTH,'#')
......@@ -17,13 +18,16 @@ data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf)
N = data['com'].shape[1]
com_pos = matlib.empty((3, N))*nan
com_vel = matlib.empty((3, N))*nan
com_acc = matlib.empty((3, N))*nan
f_RF = matlib.zeros((6, N))
f_LF = matlib.zeros((6, N))
cop_RF = matlib.zeros((2, N))
cop_LF = matlib.zeros((2, N))
N_pre = int(conf.T_pre/conf.dt)
N_post = int(conf.T_post/conf.dt)
com_pos = matlib.empty((3, N+N_post))*nan
com_vel = matlib.empty((3, N+N_post))*nan
com_acc = matlib.empty((3, N+N_post))*nan
f_RF = matlib.zeros((6, N+N_post))
f_LF = matlib.zeros((6, N+N_post))
cop_RF = matlib.zeros((2, N+N_post))
cop_LF = matlib.zeros((2, N+N_post))
contact_phase = data['contact_phase']
com_pos_ref = np.asmatrix(data['com'])
......@@ -35,7 +39,7 @@ ddx_RF_ref = np.asmatrix(data['ddx_RF'])
x_LF_ref = np.asmatrix(data['x_LF'])
dx_LF_ref = np.asmatrix(data['dx_LF'])
ddx_LF_ref = np.asmatrix(data['ddx_LF'])
com_acc_des = matlib.empty((3, N))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
com_acc_des = matlib.empty((3, N+N_post))*nan # acc_des = acc_ref - Kp*pos_err - Kd*vel_err
x_rf = tsid.get_placement_RF().translation
offset = x_rf - x_RF_ref[:,0]
......@@ -44,16 +48,16 @@ for i in range(N):
x_RF_ref[:,i] += offset
x_LF_ref[:,i] += offset
t = 0.0
t = -conf.T_pre
q, v = tsid.q, tsid.v
for i in range(-2000, N):
for i in range(-N_pre, N+N_post):
time_start = time.time()
if i==0:
print "Starting to walk (remove contact left foot)"
tsid.remove_contact_LF()
elif i>0:
elif i>0 and i<N-1:
if contact_phase[i] != contact_phase[i-1]:
print "Time %.3f Changing contact phase from %s to %s"%(t, contact_phase[i-1], contact_phase[i])
if contact_phase[i] == 'left':
......@@ -65,7 +69,7 @@ for i in range(-2000, N):
if i<0:
tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
else:
elif i<N:
tsid.set_com_ref(com_pos_ref[:,i], com_vel_ref[:,i], com_acc_ref[:,i])
tsid.set_LF_3d_ref(x_LF_ref[:,i], dx_LF_ref[:,i], ddx_LF_ref[:,i])
tsid.set_RF_3d_ref(x_RF_ref[:,i], dx_RF_ref[:,i], ddx_RF_ref[:,i])
......@@ -76,7 +80,7 @@ for i in range(-2000, N):
if(sol.status!=0):
print "QP problem could not be solved! Error code:", sol.status
break
if norm(v,2)>20.0:
if norm(v,2)>40.0:
print "Time %.3f Velocities are too high, stop everything!"%(t), norm(v)
break
......@@ -103,10 +107,10 @@ for i in range(-2000, N):
if i%conf.PRINT_N == 0:
print "Time %.3f"%(t)
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
if tsid.formulation.checkContact(tsid.contactRF.name, sol) and i>=0:
print "\tnormal force %s: %.1f"%(tsid.contactRF.name.ljust(20,'.'), f_RF[2,i])
if tsid.formulation.checkContact(tsid.contactLF.name, sol):
if tsid.formulation.checkContact(tsid.contactLF.name, sol) and i>=0:
print "\tnormal force %s: %.1f"%(tsid.contactLF.name.ljust(20,'.'), f_LF[2,i])
print "\ttracking err %s: %.3f"%(tsid.comTask.name.ljust(20,'.'), norm(tsid.comTask.position_error, 2))
......@@ -121,35 +125,35 @@ for i in range(-2000, N):
if(time_spent < conf.dt): time.sleep(conf.dt-time_spent)
# PLOT STUFF
time = np.arange(0.0, N*conf.dt, conf.dt)
time = np.arange(0.0, (N+N_post)*conf.dt, conf.dt)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_pos[i,:].A1, label='CoM '+str(i))
ax[i].plot(time, com_pos_ref[i,:].A1, 'r:', label='CoM Ref '+str(i))
ax[i].plot(time[:N], com_pos_ref[i,:].A1, 'r:', label='CoM Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
#(f, ax) = plut.create_empty_figure(3,1)
#for i in range(3):
# ax[i].plot(time, com_vel[i,:].A1, label='CoM Vel '+str(i))
# ax[i].plot(time, com_vel_ref[i,:].A1, 'r:', label='CoM Vel Ref '+str(i))
# ax[i].set_xlabel('Time [s]')
# ax[i].set_ylabel('CoM Vel [m/s]')
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
#
#(f, ax) = plut.create_empty_figure(3,1)
#for i in range(3):
# ax[i].plot(time, com_acc[i,:].A1, label='CoM Acc '+str(i))
# ax[i].plot(time, com_acc_ref[i,:].A1, 'r:', label='CoM Acc Ref '+str(i))
# ax[i].plot(time, com_acc_des[i,:].A1, 'g--', label='CoM Acc Des '+str(i))
# ax[i].set_xlabel('Time [s]')
# ax[i].set_ylabel('CoM Acc [m/s^2]')
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_vel[i,:].A1, label='CoM Vel '+str(i))
ax[i].plot(time[:N], com_vel_ref[i,:].A1, 'r:', label='CoM Vel Ref '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Vel [m/s]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(3,1)
for i in range(3):
ax[i].plot(time, com_acc[i,:].A1, label='CoM Acc '+str(i))
ax[i].plot(time[:N], com_acc_ref[i,:].A1, 'r:', label='CoM Acc Ref '+str(i))
ax[i].plot(time, com_acc_des[i,:].A1, 'g--', label='CoM Acc Des '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoM Acc [m/s^2]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
(f, ax) = plut.create_empty_figure(2,1)
for i in range(2):
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment