Improve ex4: increase weight of foot tasks, add plots

exercizes/ex_4_conf.py

@@ -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
 

exercizes/ex_4_walking.py

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

exercizes/tsid_biped.py

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