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 2f7bd1d..2300d91 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 @@ -111,7 +111,7 @@ def get_planner_settings() -> walking_settings.Settings: settings.final_state_expression_type = hippopt.ExpressionType.subject_to settings.periodicity_expression_type = hippopt.ExpressionType.subject_to settings.casadi_function_options = {"cse": True} - settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True} + settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": False} settings.casadi_solver_options = { "max_iter": 4000, "linear_solver": "mumps", @@ -592,33 +592,78 @@ def get_full_output_function( input_planner=planner, ) - computed_output = full_function( - link_length_multipliers=parametric_link_length_multipliers, - link_densities=parametric_link_densities, + # computed_output = full_function( + # link_length_multipliers=parametric_link_length_multipliers, + # link_densities=parametric_link_densities, + # ) + # + # for key in computed_output: + # if isinstance(computed_output[key], cs.DM): + # computed_output[key] = computed_output[key].full().flatten() + # + # output = planner.get_variables_structure() + # output.from_dict(computed_output) + # + # humanoid_states = [s.to_humanoid_state() for s in output.system] + # left_contact_points = [s.contact_points.left for s in humanoid_states] + # 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(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.") + # input() + # + # visualizer.visualize( + # states=humanoid_states, + # timestep_s=output.dt, + # time_multiplier=1.0, + # save=False, + # file_name_stem="humanoid_walking_periodic_sensitivity", + # ) + + link_length_multipliers_symbolic = cs.MX.sym( + "link_length_multipliers", len(planner_settings.parametric_link_names) + ) + link_densities_symbolic = cs.MX.sym( + "link_densities", len(planner_settings.parametric_link_names) + ) + + symbolic_output_dict = full_function( + link_length_multipliers=link_length_multipliers_symbolic, + link_densities=link_densities_symbolic, ) - for key in computed_output: - if isinstance(computed_output[key], cs.DM): - computed_output[key] = computed_output[key].full().flatten() + symbolic_output = planner.get_variables_structure() + symbolic_output.from_dict(symbolic_output_dict) - output = planner.get_variables_structure() - output.from_dict(computed_output) + # Compute the variability of the CoM position + com_yz_positions = [s.com[1:] for s in symbolic_output.system] - humanoid_states = [s.to_humanoid_state() for s in output.system] - left_contact_points = [s.contact_points.left for s in humanoid_states] - right_contact_points = [s.contact_points.right for s in humanoid_states] + average_com_yz = sum(com_yz_positions) / len(com_yz_positions) - visualizer_settings = get_visualizer_settings(input_settings=planner_settings) - 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.") - input() + com_yz_variability = sum( + [(yz - average_com_yz) ** 2 for yz in com_yz_positions] + ) / len(com_yz_positions) - visualizer.visualize( - states=humanoid_states, - timestep_s=output.dt, - time_multiplier=1.0, - save=True, - file_name_stem="humanoid_walking_periodic_sensitivity", + com_yz_variability_function = cs.Function( + "com_yz_variability", + [link_length_multipliers_symbolic, link_densities_symbolic], + [com_yz_variability], + ["link_length_multipliers", "link_densities"], + ["com_yz_variability_length_sensitivity"], ) + + jac = com_yz_variability_function.jacobian() + + test = jac( + parametric_link_length_multipliers, + parametric_link_densities, + com_yz_variability_function( + parametric_link_length_multipliers, + parametric_link_densities, + ), + ) + + print(test)