Multiplied the forces times the mass before returning the output solu…

…tion
ami-iit · Oct 30, 2023 · 23205bd · 23205bd
1 parent d37f7d5
commit 23205bd
Show file tree

Hide file tree

Showing 2 changed files with 12 additions and 8 deletions.
diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py
@@ -110,7 +110,7 @@ def get_planner_settings() -> walking_planner.Settings:
     settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True}
     settings.casadi_solver_options = {
         "max_iter": 4000,
-        "linear_solver": "mumps",
+        "linear_solver": "ma57",
         "alpha_for_y": "dual-and-full",
         "fast_step_computation": "yes",
         "hessian_approximation": "limited-memory",
@@ -394,9 +394,7 @@ def get_references(
 
     output = planner.solve()
 
-    humanoid_states = [
-        s.to_humanoid_state(output.values.mass) for s in output.values.system
-    ]
+    humanoid_states = [s.to_humanoid_state() for s in output.values.system]
 
     print("Press [Enter] to visualize the solution.")
     input()

diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py
@@ -285,13 +285,11 @@ def __post_init__(
         self.centroidal_momentum = np.zeros(6)
         self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints)
 
-    def to_humanoid_state(self, mass: float = 1.0) -> hp_rp.HumanoidState:
+    def to_humanoid_state(self) -> hp_rp.HumanoidState:
         output = hp_rp.HumanoidState()
         output.kinematics.base = self.kinematics.base
         output.kinematics.joints = self.kinematics.joints
         output.contact_points = self.contact_points
-        for point in output.contact_points.left + output.contact_points.right:
-            point.f *= mass
         output.com = self.com
         output.centroidal_momentum = self.centroidal_momentum
         return output
@@ -1182,4 +1180,12 @@ def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None:
         self.ocp.problem.set_initial_guess(guess)
 
     def solve(self) -> hp.Output[Variables]:
-        return self.ocp.problem.solve()
+        output = self.ocp.problem.solve()
+
+        values = output.values
+
+        for s in values.system:
+            for point in s.contact_points.left + s.contact_points.right:
+                point.f *= values.mass
+
+        return output