continuing fix kino

Author: giulioturrisi

quadruped_pympc/controllers/gradient/kinodynamic/kinodynamic_nmpc.py

@@ -669,10 +669,10 @@ def set_stage_constraint(self, constraint, state, reference, contact_sequence, h
         RL_actual_foot = state["foot_RL"]
         RR_actual_foot = state["foot_RR"]
 
-        FL_reference_foot = reference["ref_foot_swing_FL"]
-        FR_reference_foot = reference["ref_foot_swing_FR"]
-        RL_reference_foot = reference["ref_foot_swing_RL"]
-        RR_reference_foot = reference["ref_foot_swing_RR"]
+        FL_reference_foot = reference["ref_foot_FL"]
+        FR_reference_foot = reference["ref_foot_FR"]
+        RL_reference_foot = reference["ref_foot_RL"]
+        RR_reference_foot = reference["ref_foot_RR"]
 
         # Take the base position and the yaw rotation matrix. This is needed to
         # express the foothold constraint in the horizontal frame
@@ -1154,6 +1154,11 @@ def perform_scaling(self, state, reference, constraint=None):
         reference["ref_foot_swing_FR"] = reference["ref_foot_swing_FR"] - state["position"]
         reference["ref_foot_swing_RL"] = reference["ref_foot_swing_RL"] - state["position"]
         reference["ref_foot_swing_RR"] = reference["ref_foot_swing_RR"] - state["position"]
+        reference["ref_foot_FL"] = reference["ref_foot_FL"] - state["position"]
+        reference["ref_foot_FR"] = reference["ref_foot_FR"] - state["position"]
+        reference["ref_foot_RL"] = reference["ref_foot_RL"] - state["position"]
+        reference["ref_foot_RR"] = reference["ref_foot_RR"] - state["position"]
+        
 
         state["foot_FL"] = state["foot_FL"] - state["position"]
         state["foot_FR"] = state["foot_FR"] - state["position"]
@@ -1326,16 +1331,16 @@ def compute_control(
         # of a foothold that is not considered at all at touchdown! In any case,
         # the height cames always from the VFA
         if FL_contact_sequence[0] == 0:
-            state["foot_FL"] = reference["ref_foot_swing_FL"][0]
+            state["foot_FL"] = reference["ref_foot_FL"][0]
 
         if FR_contact_sequence[0] == 0:
-            state["foot_FR"] = reference["ref_foot_swing_FR"][0]
+            state["foot_FR"] = reference["ref_foot_FR"][0]
 
         if RL_contact_sequence[0] == 0:
-            state["foot_RL"] = reference["ref_foot_swing_RL"][0]
+            state["foot_RL"] = reference["ref_foot_RL"][0]
 
         if RR_contact_sequence[0] == 0:
-            state["foot_RR"] = reference["ref_foot_swing_RR"][0]
+            state["foot_RR"] = reference["ref_foot_RR"][0]
 
         if self.use_integrators:
             # Compute error for integral action
@@ -1517,13 +1522,13 @@ def compute_control(
         # If in the prediction horizon, the foot is never in stance, we replicate the reference
         # to not confuse the swing controller
         if optimal_footholds_assigned[0] == False:
-            optimal_foothold[0] = reference["ref_foot_swing_FL"][0]
+            optimal_foothold[0] = reference["ref_foot_FL"][0]
         if optimal_footholds_assigned[1] == False:
-            optimal_foothold[1] = reference["ref_foot_swing_FR"][0]
+            optimal_foothold[1] = reference["ref_foot_FR"][0]
         if optimal_footholds_assigned[2] == False:
-            optimal_foothold[2] = reference["ref_foot_swing_RL"][0]
+            optimal_foothold[2] = reference["ref_foot_RL"][0]
         if optimal_footholds_assigned[3] == False:
-            optimal_foothold[3] = reference["ref_foot_swing_RR"][0]
+            optimal_foothold[3] = reference["ref_foot_RR"][0]
 
         optimal_next_state_index = 1
         optimal_next_state = self.acados_ocp_solver.get(optimal_next_state_index, "x")[0:24]
@@ -1535,13 +1540,13 @@ def compute_control(
         if status == 1 or status == 4:
             print("status", status)
             if FL_contact_sequence[0] == 0:
-                optimal_foothold[0] = reference["ref_foot_swing_FL"][0]
+                optimal_foothold[0] = reference["ref_foot_FL"][0]
             if FR_contact_sequence[0] == 0:
-                optimal_foothold[1] = reference["ref_foot_swing_FR"][0]
+                optimal_foothold[1] = reference["ref_foot_FR"][0]
             if RL_contact_sequence[0] == 0:
-                optimal_foothold[2] = reference["ref_foot_swing_RL"][0]
+                optimal_foothold[2] = reference["ref_foot_RL"][0]
             if RR_contact_sequence[0] == 0:
-                optimal_foothold[3] = reference["ref_foot_swing_RR"][0]
+                optimal_foothold[3] = reference["ref_foot_RR"][0]
 
             number_of_legs_in_stance = np.array(
                 [FL_contact_sequence[0], FR_contact_sequence[0], RL_contact_sequence[0], RR_contact_sequence[0]]