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

Improve ex4: increase weight of foot tasks, add plots

parent 4ffe22d4
......@@ -53,7 +53,7 @@ T_pre = 1.0 # simulation time before starting to walk
T_post = 1.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_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
......
......@@ -24,6 +24,14 @@ 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
x_LF = matlib.empty((3, N+N_post))*nan
dx_LF = matlib.empty((3, N+N_post))*nan
ddx_LF = matlib.empty((3, N+N_post))*nan
ddx_LF_des = matlib.empty((3, N+N_post))*nan
x_RF = matlib.empty((3, N+N_post))*nan
dx_RF = matlib.empty((3, N+N_post))*nan
ddx_RF = matlib.empty((3, N+N_post))*nan
ddx_RF_des = 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))
......@@ -92,6 +100,13 @@ for i in range(-N_pre, N+N_post):
com_vel[:,i] = tsid.robot.com_vel(tsid.formulation.data())
com_acc[:,i] = tsid.comTask.getAcceleration(dv)
com_acc_des[:,i] = tsid.comTask.getDesiredAcceleration
x_LF[:,i], dx_LF[:,i], ddx_LF[:,i] = tsid.get_LF_3d_pos_vel_acc(dv)
if not tsid.contact_LF_active:
ddx_LF_des[:,i] = tsid.leftFootTask.getDesiredAcceleration[:3]
x_RF[:,i], dx_RF[:,i], ddx_RF[:,i] = tsid.get_RF_3d_pos_vel_acc(dv)
if not tsid.contact_RF_active:
ddx_RF_des[:,i] = tsid.rightFootTask.getDesiredAcceleration[:3]
if tsid.formulation.checkContact(tsid.contactRF.name, sol):
T_RF = tsid.contactRF.getForceGeneratorMatrix
f_RF[:,i] = T_RF * tsid.formulation.getContactForce(tsid.contactRF.name, sol)
......@@ -159,6 +174,12 @@ for i in range(3):
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()
......@@ -174,4 +195,30 @@ for i in range(2):
# leg = ax[i].legend()
# leg.get_frame().set_alpha(0.5)
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()
plt.show()
......@@ -170,6 +170,20 @@ class TsidBiped:
self.sampleLF.acc(self.sample_LF_acc)
self.leftFootTask.setReference(self.sampleLF)
def get_LF_3d_pos_vel_acc(self, dv):
data = self.formulation.data()
H = self.robot.position(data, self.LF)
v = self.robot.velocity(data, self.LF)
a = self.leftFootTask.getAcceleration(dv)
return H.translation, v.linear, a[:3]
def get_RF_3d_pos_vel_acc(self, dv):
data = self.formulation.data()
H = self.robot.position(data, self.RF)
v = self.robot.velocity(data, self.RF)
a = self.rightFootTask.getAcceleration(dv)
return H.translation, v.linear, a[:3]
def remove_contact_RF(self, transition_time=0.0):
H_rf_ref = self.robot.position(self.formulation.data(), self.RF)
self.trajRF.setReference(H_rf_ref)
......
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