Dataset generation using the simulation script.

.gitignore

@@ -303,17 +303,14 @@ pyrightconfig.json
 
 
 
-quadruped_pympc/controllers/gradient/nominal/c_generated_code_gait_adaptive/
-
-
-
 
 .idea/
 
-MUJOCO_LOG.TXT
-
-MUJOCO_LOG.TXT
-
-cfg/
+# Custom ignore parts.
+*MUJOCO_LOG.TXT
+quadruped_pympc/controllers/gradient/nominal/c_generated_code_gait_adaptive/
 
+# Datasets are heavy and shouls be saved to huggingface instead of handled here.
+/datasets/
 *.pkl
+*.h5

pyproject.toml

@@ -13,7 +13,7 @@ build-backend = "hatchling.build"
 
 [project]
 name = "quadruped_pympc"
-version = "0.1.1"
+version = "0.1.2"
 description = "A model predictive controller for quadruped robots based on the single rigid body model and written in python. Gradient-based (acados) or Sampling-based (jax)."
 authors = [
     { name="Giulio Turrisi", email="[email protected]" },

quadruped_pympc/controllers/gradient/collaborative/centroidal_model_collaborative.py

@@ -3,10 +3,9 @@
 
 # Authors: Giulio Turrisi -
 
-import time
-import unittest
-import casadi as cs
+import os
 
+import casadi as cs
 import numpy as np
 from acados_template import AcadosModel
 
@@ -17,7 +16,7 @@
 import sys
 
 sys.path.append(dir_path)
-sys.path.append(dir_path + '/../../')
+sys.path.append(dir_path + "/../../")
 
 from quadruped_pympc import config
 
@@ -165,7 +164,7 @@ def __init__(self) -> None:
             self.end_effector_gain,
         )
         fd = self.forward_dynamics(self.states, self.inputs, param)
-        self.fun_forward_dynamics = cs.Function('fun_forward_dynamics', [self.states, self.inputs, param], [fd])
+        self.fun_forward_dynamics = cs.Function("fun_forward_dynamics", [self.states, self.inputs, param], [fd])
 
     def forward_dynamics(self, states: np.ndarray, inputs: np.ndarray, param: np.ndarray) -> cs.SX:
         """

quadruped_pympc/controllers/gradient/collaborative/centroidal_nmpc_collaborative.py

@@ -5,13 +5,14 @@
 from acados_template import AcadosOcp, AcadosOcpSolver
 
 ACADOS_INFTY = 1000
-from .centroidal_model_collaborative import Centroidal_Model_Collaborative
+import copy
+import os
+
+import casadi as cs
 import numpy as np
 import scipy.linalg
-import casadi as cs
-import copy
 
-import time
+from .centroidal_model_collaborative import Centroidal_Model_Collaborative
 
 import os
 
@@ -20,7 +21,7 @@
 import sys
 
 sys.path.append(dir_path)
-sys.path.append(dir_path + '/../../')
+sys.path.append(dir_path + "/../../")
 
 from quadruped_pympc import config
 
@@ -485,8 +486,8 @@ def create_friction_cone_constraints(self) -> None:
         t = np.array([1, 0, 0])
         b = np.array([0, 1, 0])
         mu = self.centroidal_model.mu_friction
-        f_max = config.mpc_params['grf_max']
-        f_min = config.mpc_params['grf_min']
+        f_max = config.mpc_params["grf_max"]
+        f_min = config.mpc_params["grf_min"]
 
         # Derivation can be found in the paper
         # "High-slope terrain locomotion for torque-controlled quadruped robots",
@@ -1220,22 +1221,22 @@ def compute_control(
             if reference['ref_linear_velocity'].shape[0] == 3:
                 yref[3:6] = reference['ref_linear_velocity']
             else:
-                yref[3:6] = reference['ref_linear_velocity'][j]
+                yref[3:6] = reference["ref_linear_velocity"][j]
 
             if reference['ref_orientation'].shape[0] == 3:
                 yref[6:9] = reference['ref_orientation']
             else:
-                yref[6:9] = reference['ref_orientation'][j]
+                yref[6:9] = reference["ref_orientation"][j]
 
             if reference['ref_angular_velocity'].shape[0] == 3:
                 yref[9:12] = reference['ref_angular_velocity']
             else:
-                yref[9:12] = reference['ref_angular_velocity'][j]
+                yref[9:12] = reference["ref_angular_velocity"][j]
 
-            yref[12:15] = reference['ref_foot_FL'][idx_ref_foot_to_assign[0]]
-            yref[15:18] = reference['ref_foot_FR'][idx_ref_foot_to_assign[1]]
-            yref[18:21] = reference['ref_foot_RL'][idx_ref_foot_to_assign[2]]
-            yref[21:24] = reference['ref_foot_RR'][idx_ref_foot_to_assign[3]]
+            yref[12:15] = reference["ref_foot_FL"][idx_ref_foot_to_assign[0]]
+            yref[15:18] = reference["ref_foot_FR"][idx_ref_foot_to_assign[1]]
+            yref[18:21] = reference["ref_foot_RL"][idx_ref_foot_to_assign[2]]
+            yref[21:24] = reference["ref_foot_RR"][idx_ref_foot_to_assign[3]]
 
             yref[30:32] = np.array([desired_force[0], desired_force[1]])
 
@@ -1311,7 +1312,7 @@ def compute_control(
 
         # Fill stance param, friction and stance proximity
         # (stance proximity will disable foothold optimization near a stance!!)
-        mu = config.mpc_params['mu']
+        mu = config.mpc_params["mu"]
         yaw = state["orientation"][2]
 
         # Stance Proximity ugly routine. Basically we disable foothold optimization
@@ -1541,13 +1542,13 @@ def compute_control(
         # Solve ocp via RTI or normal ocp
         if self.use_RTI:
             # feedback phase
-            self.acados_ocp_solver.options_set('rti_phase', 2)
+            self.acados_ocp_solver.options_set("rti_phase", 2)
             status = self.acados_ocp_solver.solve()
-            print("feedback phase time: ", self.acados_ocp_solver.get_stats('time_tot'))
+            print("feedback phase time: ", self.acados_ocp_solver.get_stats("time_tot"))
 
         else:
             status = self.acados_ocp_solver.solve()
-            print("ocp time: ", self.acados_ocp_solver.get_stats('time_tot'))
+            print("ocp time: ", self.acados_ocp_solver.get_stats("time_tot"))
 
         # Take the solution
         control = self.acados_ocp_solver.get(0, "u")

quadruped_pympc/controllers/gradient/input_rates/centroidal_model_input_rates.py

@@ -3,9 +3,10 @@
 
 # Authors: Giulio Turrisi -
 
-import numpy as np
 import casadi as cs
+import numpy as np
 from acados_template import AcadosModel
+
 from quadruped_pympc import config
 
 
@@ -157,7 +158,7 @@ def __init__(self) -> None:
             self.mass,
         )
         fd = self.forward_dynamics(self.states, self.inputs, param)
-        self.fun_forward_dynamics = cs.Function('fun_forward_dynamics', [self.states, self.inputs, param], [fd])
+        self.fun_forward_dynamics = cs.Function("fun_forward_dynamics", [self.states, self.inputs, param], [fd])
 
     def forward_dynamics(self, states: np.ndarray, inputs: np.ndarray, param: np.ndarray) -> cs.SX:
         """

quadruped_pympc/controllers/gradient/input_rates/centroidal_nmpc_input_rates.py

@@ -2,7 +2,10 @@
 
 # Authors: Giulio Turrisi -
 
+import copy
 import os
+
+import casadi as cs
 import numpy as np
 import scipy.linalg
 import casadi as cs
@@ -14,6 +17,7 @@
 
 ACADOS_INFTY = ACADOS_INFTY = 1000
 from quadruped_pympc import config
+
 from .centroidal_model_input_rates import Centroidal_Model_InputRates
 
 
@@ -47,7 +51,7 @@ def __init__(self):
 
         self.use_DDP = config.mpc_params['use_DDP']
 
-        self.verbose = config.mpc_params['verbose']
+        self.verbose = config.mpc_params["verbose"]
 
         self.previous_status = -1
         self.previous_contact_sequence = np.zeros((4, self.horizon))
@@ -249,8 +253,8 @@ def create_ocp_solver_description(self, acados_model) -> AcadosOcp:
             # ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
             ocp.solver_options.with_adaptive_levenberg_marquardt = True
 
-            ocp.cost.cost_type = 'NONLINEAR_LS'
-            ocp.cost.cost_type_e = 'NONLINEAR_LS'
+            ocp.cost.cost_type = "NONLINEAR_LS"
+            ocp.cost.cost_type_e = "NONLINEAR_LS"
             ocp.model.cost_y_expr = cs.vertcat(ocp.model.x, ocp.model.u)
             ocp.model.cost_y_expr_e = ocp.model.x
 
@@ -483,8 +487,8 @@ def create_friction_cone_constraints(self):
         t = np.array([1, 0, 0])
         b = np.array([0, 1, 0])
         mu = self.centroidal_model.mu_friction
-        f_max = config.mpc_params['grf_max']
-        f_min = config.mpc_params['grf_min']
+        f_max = config.mpc_params["grf_max"]
+        f_min = config.mpc_params["grf_min"]
 
         # Derivation can be found in the paper
         # "High-slope terrain locomotion for torque-controlled quadruped robots",
@@ -1233,14 +1237,14 @@ def compute_control(
         idx_ref_foot_to_assign = np.array([0, 0, 0, 0])
         for j in range(self.horizon):
             yref = np.zeros(shape=(self.states_dim + self.inputs_dim,))
-            yref[0:3] = reference['ref_position']
-            yref[3:6] = reference['ref_linear_velocity']
-            yref[6:9] = reference['ref_orientation']
-            yref[9:12] = reference['ref_angular_velocity']
-            yref[12:15] = reference['ref_foot_FL'][idx_ref_foot_to_assign[0]]
-            yref[15:18] = reference['ref_foot_FR'][idx_ref_foot_to_assign[1]]
-            yref[18:21] = reference['ref_foot_RL'][idx_ref_foot_to_assign[2]]
-            yref[21:24] = reference['ref_foot_RR'][idx_ref_foot_to_assign[3]]
+            yref[0:3] = reference["ref_position"]
+            yref[3:6] = reference["ref_linear_velocity"]
+            yref[6:9] = reference["ref_orientation"]
+            yref[9:12] = reference["ref_angular_velocity"]
+            yref[12:15] = reference["ref_foot_FL"][idx_ref_foot_to_assign[0]]
+            yref[15:18] = reference["ref_foot_FR"][idx_ref_foot_to_assign[1]]
+            yref[18:21] = reference["ref_foot_RL"][idx_ref_foot_to_assign[2]]
+            yref[21:24] = reference["ref_foot_RR"][idx_ref_foot_to_assign[3]]
 
             # Update the idx_ref_foot_to_assign. Every time there is a change in the contact phase
             # between 1 and 0, it means that the leg go into swing and a new reference is needed!!!
@@ -1309,7 +1313,7 @@ def compute_control(
 
         # Fill stance param, friction and stance proximity
         # (stance proximity will disable foothold optimization near a stance!!)
-        mu = config.mpc_params['mu']
+        mu = config.mpc_params["mu"]
         yaw = state["orientation"][2]
 
         # Stance Proximity ugly routine. Basically we disable foothold optimization
@@ -1519,7 +1523,7 @@ def compute_control(
         # Solve ocp via RTI or normal ocp
         if self.use_RTI:
             # feedback phase
-            self.acados_ocp_solver.options_set('rti_phase', 2)
+            self.acados_ocp_solver.options_set("rti_phase", 2)
             status = self.acados_ocp_solver.solve()
             if self.verbose:
                 print("feedback phase time: ", self.acados_ocp_solver.get_stats('time_tot'))

quadruped_pympc/controllers/gradient/kinodynamic/kinodynamic_model.py

@@ -3,10 +3,9 @@
 
 # Authors: Giulio Turrisi -
 
-import time
-import unittest
-import casadi as cs
+import os
 
+import casadi as cs
 import numpy as np
 from acados_template import AcadosModel
 
@@ -17,7 +16,7 @@
 import sys
 
 sys.path.append(dir_path)
-sys.path.append(dir_path + '/../../')
+sys.path.append(dir_path + "/../../")
 
 from liecasadi import SO3
 
@@ -29,8 +28,8 @@
 
 if use_adam:
     # ADAM import
-    from adam.casadi import KinDynComputations
     from adam import Representations
+    from adam.casadi import KinDynComputations
 else:
     # PINOCCHIO import
     import pinocchio as pin