From 20cda11090e2e8f0777f0bc8796d927f24884a84 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 17 Apr 2024 11:57:03 +0200 Subject: [PATCH] Using output instead of initial guess to update the multipliers and densities --- .../main_periodic_step_sensitivity.py | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_sensitivity.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_sensitivity.py index 9ca6d99..48928fd 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_sensitivity.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_sensitivity.py @@ -276,7 +276,7 @@ def compute_state( for k in output_pf_dict: if isinstance(output_pf_dict[k], cs.DM): - output_pf_dict[k] = np.array(output_pf_dict[k]) + output_pf_dict[k] = output_pf_dict[k].full().flatten() output_pf = pf_input.get_variables_structure() output_pf.from_dict(output_pf_dict) @@ -567,7 +567,7 @@ def get_references( for key in output_dict: if isinstance(output_dict[key], cs.DM): - output_dict[key] = np.array(output_dict[key]) + output_dict[key] = output_dict[key].full().flatten() output = planner.get_variables_structure() output.from_dict(output_dict) @@ -577,9 +577,7 @@ def get_references( right_contact_points = [s.contact_points.right for s in humanoid_states] visualizer_settings = get_visualizer_settings(input_settings=planner_settings) - planner.set_initial_guess( - planner_guess - ) # This is to update the values of multipliers and densities + planner.set_initial_guess(output) # Update the values of multipliers and densities visualizer_settings.robot_model = planner.get_adam_model() visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) print("Press [Enter] to visualize the solution.")