Skip to content
GitLab
Commit 682eb860 authored by Andrea Del Prete's avatar Andrea Del Prete
Browse files

Improve ex 4 (add torque and vel limits, add plots)

parent e479577d
Hide whitespace changes
Inline Side-by-side
exercizes/ex_4_conf.py
View file @ 682eb860
......@@ -50,12 +50,17 @@ nb_steps = 4 # number of desired walking steps
# ----------------------------------------------
dt = 0.002 # controller time step
T_pre = 1.0 # simulation time before starting to walk
T_post = 1.0 # simulation time after walking
T_post = 2.0 # simulation time after walking
w_com = 1.0 # weight of center of mass task
w_foot = 1e0 # weight of the foot motion task
w_foot = 1e0 # weight of the foot motion task
w_posture = 1e-4 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_torque_bounds = 0.0 # weight of the torque bounds
w_joint_bounds = 0.0
tau_max_scaling = 1.45 # scaling factor of torque bounds
v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
......
exercizes/ex_4_long_conf.py
View file @ 682eb860
......@@ -49,11 +49,18 @@ nb_steps = 6 # number of desired walking steps
# configuration for TSID
# ----------------------------------------------
dt = 0.002 # controller time step
T_pre = 1.0 # simulation time before starting to walk
T_post = 2.0 # simulation time after walking
w_com = 1.0 # weight of center of mass task
w_foot = 1e-1 # weight of the foot motion task
w_posture = 1e-4 # weight of joint posture task
w_forceRef = 1e-5 # weight of force regularization task
w_torque_bounds = 0.0 # weight of the torque bounds
w_joint_bounds = 0.0
tau_max_scaling = 1.55 # scaling factor of torque bounds
v_max_scaling = 0.8
kp_contact = 10.0 # proportional gain of contact constraint
kp_foot = 10.0 # proportional gain of contact constraint
......
exercizes/ex_4_walking.py
View file @ 682eb860
......@@ -13,6 +13,12 @@ print "".center(conf.LINE_WIDTH,'#')
print " Test Walking ".center(conf.LINE_WIDTH, '#')
print "".center(conf.LINE_WIDTH,'#'), '\n'
PLOT_COM = 1
PLOT_COP = 1
PLOT_FOOT_TRAJ = 0
PLOT_TORQUES = 1
PLOT_JOINT_VEL = 1
data = np.load(conf.DATA_FILE_TSID)
tsid = TsidBiped(conf)
......@@ -36,6 +42,9 @@ 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))
tau = matlib.zeros((tsid.robot.na, N+N_post))
q_log = matlib.zeros((tsid.robot.nq, N+N_post))
v_log = matlib.zeros((tsid.robot.nv, N+N_post))
contact_phase = data['contact_phase']
com_pos_ref = np.asmatrix(data['com'])
......@@ -92,7 +101,10 @@ for i in range(-N_pre, N+N_post):
print "Time %.3f Velocities are too high, stop everything!"%(t), norm(v)
break
tau = tsid.formulation.getActuatorForces(sol)
if i>0:
q_log[:,i] = q
v_log[:,i] = v
tau[:,i] = tsid.formulation.getActuatorForces(sol)
dv = tsid.formulation.getAccelerations(sol)
if i>=0:
......@@ -142,48 +154,50 @@ for i in range(-N_pre, N+N_post):
# PLOT STUFF
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[: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[: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)
if PLOT_COM:
(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[: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_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(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):
ax[i].plot(time, cop_LF[i,:].A1, label='CoP LF '+str(i))
ax[i].plot(time, cop_RF[i,:].A1, label='CoP RF '+str(i))
if i==0:
ax[i].plot([time[0], time[-1]], [-conf.lxn, -conf.lxn], ':', label='CoP Lim '+str(i))
ax[i].plot([time[0], time[-1]], [conf.lxp, conf.lxp], ':', label='CoP Lim '+str(i))
elif i==1:
ax[i].plot([time[0], time[-1]], [-conf.lyn, -conf.lyn], ':', label='CoP Lim '+str(i))
ax[i].plot([time[0], time[-1]], [conf.lyp, conf.lyp], ':', label='CoP Lim '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoP [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
if PLOT_COP:
(f, ax) = plut.create_empty_figure(2,1)
for i in range(2):
ax[i].plot(time, cop_LF[i,:].A1, label='CoP LF '+str(i))
ax[i].plot(time, cop_RF[i,:].A1, label='CoP RF '+str(i))
if i==0:
ax[i].plot([time[0], time[-1]], [-conf.lxn, -conf.lxn], ':', label='CoP Lim '+str(i))
ax[i].plot([time[0], time[-1]], [conf.lxp, conf.lxp], ':', label='CoP Lim '+str(i))
elif i==1:
ax[i].plot([time[0], time[-1]], [-conf.lyn, -conf.lyn], ':', label='CoP Lim '+str(i))
ax[i].plot([time[0], time[-1]], [conf.lyp, conf.lyp], ':', label='CoP Lim '+str(i))
ax[i].set_xlabel('Time [s]')
ax[i].set_ylabel('CoP [m]')
leg = ax[i].legend()
leg.get_frame().set_alpha(0.5)
#(f, ax) = plut.create_empty_figure(3,2)
#ax = ax.reshape((6))
......@@ -195,30 +209,59 @@ for i in range(2):
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
for i in range(3):
if PLOT_FOOT_TRAJ:
for i in range(3):
plt.figure()
plt.plot(time, x_RF[i,:].A1, label='x RF '+str(i))
plt.plot(time[:N], x_RF_ref[i,:].A1, ':', label='x RF ref '+str(i))
plt.plot(time, x_LF[i,:].A1, label='x LF '+str(i))
plt.plot(time[:N], x_LF_ref[i,:].A1, ':', label='x LF ref '+str(i))
plt.legend()
#for i in range(3):
# plt.figure()
# plt.plot(time, dx_RF[i,:].A1, label='dx RF '+str(i))
# plt.plot(time[:N], dx_RF_ref[i,:].A1, ':', label='dx RF ref '+str(i))
# plt.plot(time, dx_LF[i,:].A1, label='dx LF '+str(i))
# plt.plot(time[:N], dx_LF_ref[i,:].A1, ':', label='dx LF ref '+str(i))
# plt.legend()
#
#for i in range(3):
# plt.figure()
# plt.plot(time, ddx_RF[i,:].A1, label='ddx RF '+str(i))
# plt.plot(time[:N], ddx_RF_ref[i,:].A1, ':', label='ddx RF ref '+str(i))
# plt.plot(time, ddx_RF_des[i,:].A1, '--', label='ddx RF des '+str(i))
# plt.plot(time, ddx_LF[i,:].A1, label='ddx LF '+str(i))
# plt.plot(time[:N], ddx_LF_ref[i,:].A1, ':', label='ddx LF ref '+str(i))
# plt.plot(time, ddx_LF_des[i,:].A1, '--', label='ddx LF des '+str(i))
# plt.legend()
if(PLOT_TORQUES):
plt.figure()
plt.plot(time, x_RF[i,:].A1, label='x RF '+str(i))
plt.plot(time[:N], x_RF_ref[i,:].A1, ':', label='x RF ref '+str(i))
plt.plot(time, x_LF[i,:].A1, label='x LF '+str(i))
plt.plot(time[:N], x_LF_ref[i,:].A1, ':', label='x LF ref '+str(i))
plt.legend()
for i in range(tsid.robot.na):
tau_normalized = 2*(tau[i,:].A1-tsid.tau_min[i,0]) / (tsid.tau_max[i,0]-tsid.tau_min[i,0]) - 1
# plot torques only for joints that reached 50% of max torque
if np.max(np.abs(tau_normalized))>0.5:
plt.plot(time, tau_normalized, alpha=0.5, label=tsid.model.names[i+2])
plt.plot([time[0], time[-1]], 2*[-1.0], ':')
plt.plot([time[0], time[-1]], 2*[1.0], ':')
plt.gca().set_xlabel('Time [s]')
plt.gca().set_ylabel('Normalized Torque')
leg = plt.legend()
leg.get_frame().set_alpha(0.5)
#for i in range(3):
# plt.figure()
# plt.plot(time, dx_RF[i,:].A1, label='dx RF '+str(i))
# plt.plot(time[:N], dx_RF_ref[i,:].A1, ':', label='dx RF ref '+str(i))
# plt.plot(time, dx_LF[i,:].A1, label='dx LF '+str(i))
# plt.plot(time[:N], dx_LF_ref[i,:].A1, ':', label='dx LF ref '+str(i))
# plt.legend()
#
#for i in range(3):
# plt.figure()
# plt.plot(time, ddx_RF[i,:].A1, label='ddx RF '+str(i))
# plt.plot(time[:N], ddx_RF_ref[i,:].A1, ':', label='ddx RF ref '+str(i))
# plt.plot(time, ddx_RF_des[i,:].A1, '--', label='ddx RF des '+str(i))
# plt.plot(time, ddx_LF[i,:].A1, label='ddx LF '+str(i))
# plt.plot(time[:N], ddx_LF_ref[i,:].A1, ':', label='ddx LF ref '+str(i))
# plt.plot(time, ddx_LF_des[i,:].A1, '--', label='ddx LF des '+str(i))
# plt.legend()
if(PLOT_JOINT_VEL):
plt.figure()
for i in range(tsid.robot.na):
v_normalized = 2*(v_log[6+i,:].A1-tsid.v_min[i,0]) / (tsid.v_max[i,0]-tsid.v_min[i,0]) - 1
# plot v only for joints that reached 50% of max v
if np.max(np.abs(v_normalized))>0.5:
plt.plot(time, v_normalized, alpha=0.5, label=tsid.model.names[i+2])
plt.plot([time[0], time[-1]], 2*[-1.0], ':')
plt.plot([time[0], time[-1]], 2*[1.0], ':')
plt.gca().set_xlabel('Time [s]')
plt.gca().set_ylabel('Normalized Joint Vel')
leg = plt.legend()
leg.get_frame().set_alpha(0.5)
plt.show()
exercizes/plot_utils.py
View file @ 682eb860
......@@ -78,6 +78,7 @@ mpl.rcdefaults()
mpl.rcParams['lines.linewidth'] = DEFAULT_LINE_WIDTH;
mpl.rcParams['lines.markersize'] = DEFAULT_MARKER_SIZE;
mpl.rcParams['patch.linewidth'] = 1;
mpl.rcParams['axes.grid'] = True
mpl.rcParams['font.family'] = DEFAULT_FONT_FAMILY;
mpl.rcParams['font.size'] = DEFAULT_FONT_SIZE;
mpl.rcParams['font.serif'] = DEFAULT_FONT_SERIF;
......
exercizes/tsid_biped.py
View file @ 682eb860
......@@ -84,6 +84,20 @@ class TsidBiped:
self.trajRF = tsid.TrajectorySE3Constant("traj-right-foot", H_rf_ref)
formulation.addMotionTask(self.rightFootTask, self.conf.w_foot, 1, 0.0)
self.tau_max = conf.tau_max_scaling*robot.model().effortLimit[-robot.na:]
self.tau_min = -self.tau_max
actuationBoundsTask = tsid.TaskActuationBounds("task-actuation-bounds", robot)
actuationBoundsTask.setBounds(self.tau_min, self.tau_max)
if(conf.w_torque_bounds>0.0):
formulation.addActuationTask(actuationBoundsTask, conf.w_torque_bounds, 0, 0.0)
jointBoundsTask = tsid.TaskJointBounds("task-joint-bounds", robot, conf.dt)
self.v_max = conf.v_max_scaling * robot.model().velocityLimit[-robot.na:]
self.v_min = -self.v_max
jointBoundsTask.setVelocityBounds(self.v_min, self.v_max)
if(conf.w_joint_bounds>0.0):
formulation.addMotionTask(jointBoundsTask, conf.w_joint_bounds, 0, 0.0)
com_ref = robot.com(data)
self.trajCom = tsid.TrajectoryEuclidianConstant("traj_com", com_ref)
self.sample_com = self.trajCom.computeNext()
......@@ -109,6 +123,8 @@ class TsidBiped:
self.postureTask = postureTask
self.contactRF = contactRF
self.contactLF = contactLF
self.actuationBoundsTask = actuationBoundsTask
self.jointBoundsTask = jointBoundsTask
self.formulation = formulation
self.q = q
self.v = v
......
Supports Markdown
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