[py] Add angular momentum task to tsid-biped, which improves the behavior during walking

exercizes/ex_4_conf.py

@@ -57,6 +57,7 @@ T_post = 1.5                    # simulation time after walking
 
 w_com = 1.0                     # weight of center of mass task
 w_cop = 0.0                     # weight of center of pressure task
+w_am = 1e-4                     # weight of angular momentum task
 w_foot = 1e0                    # weight of the foot motion task
 w_contact = 1e2                 # weight of the foot in contact
 w_posture = 1e-4                # weight of joint posture task
@@ -70,6 +71,7 @@ v_max_scaling = 0.8
 kp_contact = 10.0               # proportional gain of contact constraint
 kp_foot = 10.0                  # proportional gain of contact constraint
 kp_com = 10.0                   # proportional gain of center of mass task
+kp_am = 10.0                   # proportional gain of angular momentum task
 kp_posture = 1.0               # proportional gain of joint posture task
 gain_vector = kp_posture*np.ones(nv-6)
 masks_posture = np.ones(nv-6)

exercizes/ex_4_walking.py

@@ -84,6 +84,7 @@ for i in range(-N_pre, N+N_post):
                 tsid.add_contact_RF()
                 tsid.remove_contact_LF()
     
+    
     if i<0:
         tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])
     elif i<N:

exercizes/romeo_conf.py

@@ -30,6 +30,7 @@ contactNormal = np.array([0., 0., 1.])   # direction of the normal to the contac
 
 w_com = 1.0                     # weight of center of mass task
 w_cop = 0.0                     # weight of center of pressure task
+w_am = 0.0                      # weight of angular momentum task
 w_foot = 1e-1                   # weight of the foot motion task
 w_contact = -1.0                # weight of foot in contact (negative means infinite weight) 
 w_posture = 1e-4                # weight of joint posture task
@@ -43,6 +44,7 @@ v_max_scaling = 0.8
 kp_contact = 10.0               # proportional gain of contact constraint
 kp_foot = 10.0                  # proportional gain of contact constraint
 kp_com = 10.0                   # proportional gain of center of mass task
+kp_am = 10.0                   # proportional gain of angular momentum task
 kp_posture = 1.0               # proportional gain of joint posture task
 gain_vector = kp_posture*np.ones(nv-6)
 masks_posture = np.ones(nv-6)

exercizes/test_cop_task.py

@@ -88,7 +88,7 @@ for i in range(-N_pre, N+N_post):
         print("Starting to walk (remove contact left foot)")
         tsid.remove_contact_LF()
         # activate CoP task only when robot starts walking
-        tsid.formulation.updateTaskWeight(tsid.copTask.name, 1e-3)
+        tsid.formulation.updateTaskWeight(tsid.copTask.name, 1e-4)
     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]))
@@ -98,6 +98,12 @@ for i in range(-N_pre, N+N_post):
             else:
                 tsid.add_contact_RF()
                 tsid.remove_contact_LF()
+    elif i==N:
+        # switch to double support at the end
+        if contact_phase[i-1] == 'left':
+            tsid.add_contact_RF()
+        else:
+            tsid.add_contact_LF()
     
     if i<0:
         tsid.set_com_ref(com_pos_ref[:,0], 0*com_vel_ref[:,0], 0*com_acc_ref[:,0])

exercizes/tsid_biped.py

@@ -71,6 +71,14 @@ class TsidBiped:
         
         copTask = tsid.TaskCopEquality("task-cop", robot)
         formulation.addForceTask(copTask, conf.w_cop, 1, 0.0)
+        
+        amTask = tsid.TaskAMEquality("task-am", robot)
+        amTask.setKp(conf.kp_am * np.array([1., 1., 0.]))
+        amTask.setKd(2.0 * np.sqrt(conf.kp_am * np.array([1., 1., 0.])))
+        formulation.addMotionTask(amTask, conf.w_am, 1, 0.)
+        sampleAM = tsid.TrajectorySample(3)
+        amTask.setReference(sampleAM)
+
 
         postureTask = tsid.TaskJointPosture("task-posture", robot)
         postureTask.setKp(conf.kp_posture * conf.gain_vector)
@@ -127,6 +135,7 @@ class TsidBiped:
 
         self.comTask = comTask
         self.copTask = copTask
+        self.amTask = amTask
         self.postureTask = postureTask
         self.contactRF = contactRF
         self.contactLF = contactLF