deg fix

pyomeca · Oct 29, 2024 · f145d20 · f145d20
1 parent 8bb744b
commit f145d20
Show file tree

Hide file tree

Showing 2 changed files with 19 additions and 3 deletions.
diff --git a/bioptim/dynamics/configure_problem.py b/bioptim/dynamics/configure_problem.py
@@ -1211,7 +1211,22 @@ def configure_soft_contact_function(ocp, nlp):
             A reference to the phase
         """
         component_list = ["Mx", "My", "Mz", "Fx", "Fy", "Fz"]
-        nlp.soft_contact_forces_func = nlp.model.soft_contact_forces()
+
+        global_soft_contact_force_func = nlp.model.soft_contact_forces()(
+            nlp.states["q"].cx_start, nlp.states["qdot"].cx_start, nlp.parameters.cx
+        )
+        nlp.soft_contact_forces_func = Function(
+            "soft_contact_forces_func",
+            [
+                nlp.time_cx,
+                nlp.states.cx_start,
+                nlp.controls.cx_start,
+                nlp.parameters.cx_start,
+            ],
+            [global_soft_contact_force_func],
+            ["t", "x", "u", "p"],
+            ["soft_contact_forces"],
+        ).expand()
 
         for i_sc in range(nlp.model.nb_soft_contacts):
             all_soft_contact_names = []

diff --git a/bioptim/limits/penalty.py b/bioptim/limits/penalty.py
@@ -863,8 +863,9 @@ def minimize_soft_contact_forces(
                 force_idx.append(4 + (6 * i_sc))
                 force_idx.append(5 + (6 * i_sc))
             soft_contact_force = controller.get_nlp.soft_contact_forces_func(
-                controller.q,
-                controller.qdot,
+                controller.time.cx,
+                controller.states.cx_start,
+                controller.controls.cx_start,
                 controller.parameters.cx,
             )
             return soft_contact_force[force_idx]