Allow using the opti callback in the kinodynamic planner

ami-iit · Jan 19, 2024 · 2d49244 · 2d49244
1 parent d9985aa
commit 2d49244
Show file tree

Hide file tree

Showing 2 changed files with 22 additions and 0 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py
@@ -34,11 +34,21 @@ def __init__(self, settings: Settings) -> None:
             settings=self.settings, kin_dyn_object=self.kin_dyn_object
         )
 
+        opti_callback = None
+        if self.settings.use_opti_callback:
+            opti_callback = (
+                hp.opti_callback.BestCost()
+                & hp.opti_callback.AcceptablePrimalInfeasibility(
+                    self.settings.acceptable_constraint_violation
+                )
+            )
+
         optimization_solver = hp.OptiSolver(
             inner_solver=self.settings.opti_solver,
             problem_type=self.settings.problem_type,
             options_solver=self.settings.casadi_solver_options,
             options_plugin=self.settings.casadi_opti_options,
+            callback_criterion=opti_callback,
         )
         ocp_solver = hp.MultipleShootingSolver(optimization_solver=optimization_solver)
 

diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py
@@ -70,6 +70,14 @@ class Settings:
 
     problem_type: str = dataclasses.field(default="nlp")
 
+    use_opti_callback: bool = dataclasses.field(default=None)
+
+    acceptable_constraint_violation: float = dataclasses.field(default=None)
+
+    opti_callback_save_costs: bool = dataclasses.field(default=None)
+
+    opti_callback_save_constraint_multipliers: bool = dataclasses.field(default=None)
+
     casadi_function_options: dict = dataclasses.field(default_factory=dict)
 
     casadi_opti_options: dict = dataclasses.field(default_factory=dict)
@@ -92,6 +100,10 @@ def __post_init__(self) -> None:
         self.final_state_expression_weight = 1.0
         self.periodicity_expression_type = hp.ExpressionType.skip
         self.periodicity_expression_weight = 1.0
+        self.use_opti_callback = False
+        self.acceptable_constraint_violation = 1e-3
+        self.opti_callback_save_costs = True
+        self.opti_callback_save_constraint_multipliers = True
 
     def is_valid(self) -> bool:
         number_of_joints = len(self.joints_name_list)