From 2d4924427c032f4e5fceec90e3f86d381a0a7ae3 Mon Sep 17 00:00:00 2001
From: Stefano Dafarra <stefano.dafarra@gmail.com>
Date: Tue, 9 Jan 2024 14:39:35 +0100
Subject: [PATCH] Allow using the opti callback in the kinodynamic planner

---
 .../turnkey_planners/humanoid_kinodynamic/planner.py | 10 ++++++++++
 .../humanoid_kinodynamic/settings.py                 | 12 ++++++++++++
 2 files changed, 22 insertions(+)

diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py
index 218a523e..43a36ba7 100644
--- 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
index 4b139872..51d15413 100644
--- 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)