Authorize longer lines with black (up to 120 char)

inria-paris-robotics-lab · Feb 15, 2024 · 37f93ee · 37f93ee
1 parent aae0727
commit 37f93ee
Show file tree

Hide file tree

Showing 23 changed files with 106 additions and 344 deletions.
diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml
@@ -27,6 +27,7 @@ repos:
     rev: 23.9.1
     hooks:
     -   id: black
+        args: ["--line-length", "120"]
 -   repo: https://github.com/charliermarsh/ruff-pre-commit
     # Ruff version.
     rev: v0.0.292

diff --git a/compare_logs.py b/compare_logs.py
@@ -93,11 +93,7 @@ def infNorm(x, **kwargs):
 plt.yscale("log")
 plt.legend()
 plt.grid(which="both")
-plt.title(
-    "Comparison between {:s} and {:s}".format(
-        get_solver_name(data1), get_solver_name(data2)
-    )
-)
+plt.title("Comparison between {:s} and {:s}".format(get_solver_name(data1), get_solver_name(data2)))
 
 # Error over time & mpc iteration
 Xerr_over_time = infNorm(Xerr[::WBC_RATIO], axis=2)

diff --git a/examples/bench_croc_ocp.py b/examples/bench_croc_ocp.py
@@ -75,23 +75,9 @@ def runShootingProblemCalcDiffBenchmark(xs, us, problem):
 print("Python bindings:")
 xs, us, problem = createProblem()
 avrg_duration, min_duration, max_duration = runDDPSolveBenchmark(xs, us, problem)
-print(
-    "  DDP.solve [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration)
-)
-avrg_duration, min_duration, max_duration = runShootingProblemCalcBenchmark(
-    xs, us, problem
-)
-print(
-    "  ShootingProblem.calc [ms]: {0} ({1}, {2})".format(
-        avrg_duration, min_duration, max_duration
-    )
-)
-avrg_duration, min_duration, max_duration = runShootingProblemCalcDiffBenchmark(
-    xs, us, problem
-)
-print(
-    "  ShootingProblem.calcDiff [ms]: {0} ({1}, {2})".format(
-        avrg_duration, min_duration, max_duration
-    )
-)
+print("  DDP.solve [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration))
+avrg_duration, min_duration, max_duration = runShootingProblemCalcBenchmark(xs, us, problem)
+print("  ShootingProblem.calc [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration))
+avrg_duration, min_duration, max_duration = runShootingProblemCalcDiffBenchmark(xs, us, problem)
+print("  ShootingProblem.calcDiff [ms]: {0} ({1}, {2})".format(avrg_duration, min_duration, max_duration))
 print("\033[0m")
diff --git a/python/quadruped_reactive_walking/controller.py b/python/quadruped_reactive_walking/controller.py
@@ -63,9 +63,7 @@ class Controller:
     t_mpc = 0.0
     q_security = np.array([1.2, 2.1, 3.14] * 4)
 
-    def __init__(
-        self, params: qrw.Params, q_init, solver_cls: Type[wb_mpc.OCPAbstract]
-    ):
+    def __init__(self, params: qrw.Params, q_init, solver_cls: Type[wb_mpc.OCPAbstract]):
         """
         Function that computes the reference control (tau, q_des, v_des and gains)
 
@@ -98,9 +96,7 @@ def __init__(
         self.result.v_des = self.task.v0[6:].copy()
 
         self.target = Target(params)
-        self.footsteps, self.base_refs = make_footsteps_and_refs(
-            self.params, self.target
-        )
+        self.footsteps, self.base_refs = make_footsteps_and_refs(self.params, self.target)
 
         self.default_footstep = make_initial_footstep(params.q_init)
         self.target_base = pin.Motion.Zero()
@@ -134,15 +130,11 @@ def _create_mpc(self, solver_cls):
             if self.params.asynchronous_mpc:
                 from .wbmpc_wrapper_ros_mp import ROSMPCAsyncClient
 
-                return ROSMPCAsyncClient(
-                    self.params, self.footsteps, self.base_refs, solver_cls
-                )
+                return ROSMPCAsyncClient(self.params, self.footsteps, self.base_refs, solver_cls)
             else:
                 from .wbmpc_wrapper_ros import ROSMPCWrapperClient
 
-                return ROSMPCWrapperClient(
-                    self.params, self.footsteps, self.base_refs, solver_cls, True
-                )
+                return ROSMPCWrapperClient(self.params, self.footsteps, self.base_refs, solver_cls, True)
         else:
             if self.params.asynchronous_mpc:
                 from .wbmpc_wrapper_multiprocess import (
@@ -181,9 +173,7 @@ def compute(self, device, qc=None):
             self.target_footstep[:] = 0.0
         else:
             self.target_base.np[:] = 0.0
-            self.target_footstep[:] = self.target.compute(
-                self.k + self.params.N_gait * self.params.mpc_wbc_ratio
-            )
+            self.target_footstep[:] = self.target.compute(self.k + self.params.N_gait * self.params.mpc_wbc_ratio)
 
         if self.k % self.params.mpc_wbc_ratio == 0:
             if self.mpc_solved:
@@ -197,9 +187,7 @@ def compute(self, device, qc=None):
 
             try:
                 self.t_mpc_start = time.time()
-                self.mpc.solve(
-                    self.k, x, self.target_footstep.copy(), self.target_base.copy()
-                )
+                self.mpc.solve(self.k, x, self.target_footstep.copy(), self.target_base.copy())
             except ValueError:
                 import traceback
 
@@ -301,14 +289,10 @@ def clamp_result(self, device, set_error=False):
             if self.clamp(self.result.q_des[3 * i + 1], -hip_max, hip_max):
                 hip_ids.append(i)
                 self.error = set_error
-            if self.task.q0[7 + 3 * i + 2] >= 0.0 and self.clamp(
-                self.result.q_des[3 * i + 2], knee_min
-            ):
+            if self.task.q0[7 + 3 * i + 2] >= 0.0 and self.clamp(self.result.q_des[3 * i + 2], knee_min):
                 knee_ids.append(i)
                 self.error = set_error
-            elif self.task.q0[7 + 3 * i + 2] <= 0.0 and self.clamp(
-                self.result.q_des[3 * i + 2], max_value=-knee_min
-            ):
+            elif self.task.q0[7 + 3 * i + 2] <= 0.0 and self.clamp(self.result.q_des[3 * i + 2], max_value=-knee_min):
                 knee_ids.append(i)
                 self.error = set_error
         if len(hip_ids) > 0:
@@ -365,9 +349,7 @@ def save_guess(self, filename="/tmp/init_guess.npy"):
         )
         print("Initial guess saved")
 
-    def run_estimator(
-        self, device, q_perfect=np.zeros(6), b_baseVel_perfect=np.zeros(3)
-    ):
+    def run_estimator(self, device, q_perfect=np.zeros(6), b_baseVel_perfect=np.zeros(3)):
         """
         Call the estimator and retrieve the reference and estimated quantities.
         Run a filter on q, h_v and v_ref.
@@ -418,9 +400,7 @@ def run_estimator(
 
         self.q_filtered = self.q_estimate.copy()
         self.q_filtered[:3] = self.base_position_filtered[:3]
-        self.q_filtered[3:7] = pin.Quaternion(
-            pin.rpy.rpyToMatrix(self.base_position_filtered[3:])
-        ).coeffs()
+        self.q_filtered[3:7] = pin.Quaternion(pin.rpy.rpyToMatrix(self.base_position_filtered[3:])).coeffs()
         self.v_filtered = self.v_estimate.copy()
         # self.v_filtered[:6] = np.zeros(6)
         # self.v_filtered[:6] = self.filter_v.filter(self.v_windowed, False)

diff --git a/python/quadruped_reactive_walking/main_solo12_control.py b/python/quadruped_reactive_walking/main_solo12_control.py
@@ -188,9 +188,7 @@ def main(args):
     device, qc = get_device(params.SIMULATION, sim_params["record_video"])
 
     if params.LOGGING or params.PLOTTING:
-        logger = LoggerControl(
-            params, log_size=params.N_SIMULATION, solver_cls_name=args.solver
-        )
+        logger = LoggerControl(params, log_size=params.N_SIMULATION, solver_cls_name=args.solver)
     else:
         logger = None
 
@@ -217,9 +215,7 @@ def main(args):
     T_whole = time.time()
     dT_whole = 0.0
     disable = params.ocp.verbose
-    bar_format = (
-        "{desc}: {percentage:.4f}%|{bar}| {n:.3f}/{total:.3f} [{elapsed}<{remaining}]"
-    )
+    bar_format = "{desc}: {percentage:.4f}%|{bar}| {n:.3f}/{total:.3f} [{elapsed}<{remaining}]"
     with tqdm.tqdm(
         desc="MPC cycles",
         total=t_max + params.dt_wbc,
@@ -251,9 +247,7 @@ def main(args):
             device.joints.set_velocity_gains(controller.result.D)
             device.joints.set_desired_positions(controller.result.q_des)
             device.joints.set_desired_velocities(controller.result.v_des)
-            device.joints.set_torques(
-                controller.result.FF_weight * controller.result.tau_ff.ravel()
-            )
+            device.joints.set_torques(controller.result.FF_weight * controller.result.tau_ff.ravel())
             device.send_command_and_wait_end_of_cycle(params.dt_wbc)
 
             if params.LOGGING or params.PLOTTING:

diff --git a/python/quadruped_reactive_walking/ocp_defs/walking.py b/python/quadruped_reactive_walking/ocp_defs/walking.py
@@ -38,12 +38,8 @@ def __init__(self, params: Params, footsteps, base_vel_refs):
             axis=0,
         )
 
-        self.life_rm, self.life_tm = self.initialize_models_from_gait(
-            self.life_gait, footsteps, base_vel_refs
-        )
-        self.start_rm, self.start_tm = self.initialize_models_from_gait(
-            self.starting_gait
-        )
+        self.life_rm, self.life_tm = self.initialize_models_from_gait(self.life_gait, footsteps, base_vel_refs)
+        self.start_rm, self.start_tm = self.initialize_models_from_gait(self.starting_gait)
         self.end_rm, self.end_tm = self.initialize_models_from_gait(self.ending_gait)
 
         self.x0 = self.task.x0
@@ -68,12 +64,7 @@ def select_next_model(self, k, current_gait, base_vel_ref):
             no_copy_roll_insert(current_gait, self.life_gait[-1])
 
         else:
-            i = (
-                0
-                if t
-                == len(self.start_rm) + len(self.life_rm) * self.params.gait_repetitions
-                else 1
-            )
+            i = 0 if t == len(self.start_rm) + len(self.life_rm) * self.params.gait_repetitions else 1
             # choose to pich the node with impact or not
             support_feet = feet_ids[self.ending_gait[i] == 1]
             model = self.end_rm[i]
@@ -98,29 +89,19 @@ def initialize_models_from_gait(self, gait, footsteps=None, base_vel_refs=None):
         feet_ids = np.asarray(self.task.feet_ids)
         for t in range(gait.shape[0]):
             support_feet_ids = feet_ids[gait[t] == 1]
-            feet_pos = (
-                get_active_feet(footsteps[t], support_feet_ids)
-                if footsteps is not None
-                else []
-            )
+            feet_pos = get_active_feet(footsteps[t], support_feet_ids) if footsteps is not None else []
             base_vel_ref = base_vel_refs[t] if base_vel_refs is not None else None
             has_switched = np.any(gait[t] != gait[t - 1])
             switch_matrix = gait[t] if has_switched else np.array([])
             switch_feet = feet_ids[switch_matrix == 1]
-            running_models.append(
-                self.make_running_model(
-                    support_feet_ids, switch_feet, feet_pos, base_vel_ref
-                )
-            )
+            running_models.append(self.make_running_model(support_feet_ids, switch_feet, feet_pos, base_vel_ref))
 
         support_feet_ids = feet_ids[gait[-1] == 1]
         terminal_model = self.make_terminal_model(support_feet_ids)
 
         return running_models, terminal_model
 
-    def _create_standard_model(
-        self, support_feet
-    ) -> crocoddyl.IntegratedActionModelAbstract:
+    def _create_standard_model(self, support_feet) -> crocoddyl.IntegratedActionModelAbstract:
         """
         Create a standard integrated action model, to be modified by the callee.
 
@@ -160,14 +141,10 @@ def _create_standard_model(
             ActivationBounds(-self.task.state_limit, self.task.state_limit),
             self.task.state_bound_w**2,
         )
-        state_bound_cost = CostModelResidual(
-            self.state, activation, state_bound_residual
-        )
+        state_bound_cost = CostModelResidual(self.state, activation, state_bound_residual)
         costs.addCost("state_limitBound", state_bound_cost, 1)
 
-        diff = DifferentialActionModelContactFwdDynamics(
-            self.state, actuation, contacts, costs, 0.0, True
-        )
+        diff = DifferentialActionModelContactFwdDynamics(self.state, actuation, contacts, costs, 0.0, True)
         return IntegratedActionModelEuler(diff, self.params.dt_mpc)
 
     def make_running_model(
@@ -213,17 +190,13 @@ def make_running_model(
 
     def _add_control_costs(self, costs: CostModelSum):
         nu = costs.nu
-        control_reg = CostModelResidual(
-            self.state, ResidualModelControl(self.state, self.task.uref)
-        )
+        control_reg = CostModelResidual(self.state, ResidualModelControl(self.state, self.task.uref))
         costs.addCost("control_reg", control_reg, self.task.control_reg_w)
 
         control_bound_activation = crocoddyl.ActivationModelQuadraticBarrier(
             ActivationBounds(-self.task.effort_limit, self.task.effort_limit)
         )
-        control_bound = CostModelResidual(
-            self.state, control_bound_activation, ResidualModelControl(self.state, nu)
-        )
+        control_bound = CostModelResidual(self.state, control_bound_activation, ResidualModelControl(self.state, nu))
         costs.addCost("control_bound", control_bound, self.task.control_bound_w)
 
     def make_terminal_model(self, support_feet):
@@ -241,13 +214,9 @@ def make_terminal_model(self, support_feet):
     def _add_friction_cost(self, i: int, support_feet, costs: CostModelSum):
         nu = costs.nu
         # Contact forces
-        cone = crocoddyl.FrictionCone(
-            self.task.Rsurf, self.task.friction_mu, 4, False, 3
-        )
+        cone = crocoddyl.FrictionCone(self.task.Rsurf, self.task.friction_mu, 4, False, 3)
         residual = crocoddyl.ResidualModelContactFrictionCone(self.state, i, cone, nu)
-        activation = crocoddyl.ActivationModelQuadraticBarrier(
-            ActivationBounds(cone.lb, cone.ub)
-        )
+        activation = crocoddyl.ActivationModelQuadraticBarrier(ActivationBounds(cone.lb, cone.ub))
         friction_cost = CostModelResidual(self.state, activation, residual)
         friction_name = self.rmodel.frames[i].name + "_friction_cost"
         costs.addCost(friction_name, friction_cost, self.task.friction_cone_w)
@@ -275,9 +244,7 @@ def _add_foot_track_cost(self, i: int, costs: CostModelSum):
         nu = costs.nu
         # Tracking foot trajectory
         name = self.rmodel.frames[i].name + "_foot_tracking"
-        residual = crocoddyl.ResidualModelFrameTranslation(
-            self.state, i, np.zeros(3), nu
-        )
+        residual = crocoddyl.ResidualModelFrameTranslation(self.state, i, np.zeros(3), nu)
         foot_tracking = CostModelResidual(self.state, residual)
         costs.addCost(name, foot_tracking, self.task.foot_tracking_w)
         costs.changeCostStatus(name, False)
@@ -286,17 +253,11 @@ def _add_ground_coll_penalty(self, i: int, costs: CostModelSum, start_pos):
         nu = costs.nu
 
         # Swing foot
-        ground_coll_res = crocoddyl.ResidualModelFrameTranslation(
-            self.state, i, start_pos, nu
-        )
+        ground_coll_res = crocoddyl.ResidualModelFrameTranslation(self.state, i, start_pos, nu)
 
-        bounds = ActivationBounds(
-            np.array([-1000, -1000, start_pos[2]]), np.array([1000, 1000, 1000])
-        )
+        bounds = ActivationBounds(np.array([-1000, -1000, start_pos[2]]), np.array([1000, 1000, 1000]))
         ground_coll_activ = crocoddyl.ActivationModelQuadraticBarrier(bounds)
-        ground_coll_cost = CostModelResidual(
-            self.state, ground_coll_activ, ground_coll_res
-        )
+        ground_coll_cost = CostModelResidual(self.state, ground_coll_activ, ground_coll_res)
 
         name = "{}_groundCol".format(self.rmodel.frames[i].name)
         costs.addCost(
@@ -337,9 +298,7 @@ def _add_vert_velocity_cost(self, i: int, costs: CostModelSum):
             pin.ReferenceFrame.WORLD,
             nu,
         )
-        vertical_velocity_activation = ActivationModelWeightedQuad(
-            np.array([0, 0, 1, 0, 0, 0])
-        )
+        vertical_velocity_activation = ActivationModelWeightedQuad(np.array([0, 0, 1, 0, 0, 0]))
 
         name = "{}_vel_zReg".format(self.rmodel.frames[i].name)
         vertical_velocity_reg_cost = CostModelResidual(
@@ -420,13 +379,9 @@ def update_model(
             name = self.rmodel.frames[i].name + "_contact"
             model.differential.contacts.changeContactStatus(name, i in support_feet)
         if not is_terminal:
-            self.update_tracking_costs(
-                model.differential.costs, feet_pos, base_vel_ref, support_feet
-            )
+            self.update_tracking_costs(model.differential.costs, feet_pos, base_vel_ref, support_feet)
 
-    def update_tracking_costs(
-        self, costs, feet_pos: List[np.ndarray], base_vel_ref: pin.Motion, support_feet
-    ):
+    def update_tracking_costs(self, costs, feet_pos: List[np.ndarray], base_vel_ref: pin.Motion, support_feet):
         index = 0
         for i in self.task.feet_ids:
             if self.has_foot_track_cost: