From e602b5a06586a5abc47383fba05b904f5d2ad86e Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 7 Jul 2024 12:31:44 +0200 Subject: [PATCH] Added complex pose with both hands reaching the ground --- .../main_complex_poses.py | 60 ++++++++++++++++++- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main_complex_poses.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main_complex_poses.py index a0eb2fd..14c8612 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main_complex_poses.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main_complex_poses.py @@ -43,6 +43,11 @@ def get_planner_settings( use_joint_limits: bool = True, constrain_left_foot_position: bool = False, constrain_right_foot_position: bool = False, + torso_orientation_scaling: float = 100.0, + left_hand_expression_type: hippopt.ExpressionType = hippopt.ExpressionType.skip, + left_hand_scaling: float = 1.0, + right_hand_expression_type: hippopt.ExpressionType = hippopt.ExpressionType.skip, + right_hand_scaling: float = 1.0, ) -> pose_finder.Settings: urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" @@ -94,7 +99,7 @@ def get_planner_settings( output_settings.joint_regularization_cost_weights[3:11] = 10.0 # arms output_settings.joint_regularization_cost_weights[11:] = 1.0 # legs output_settings.base_quaternion_cost_multiplier = 50.0 - output_settings.desired_frame_quaternion_cost_multiplier = 100.0 + output_settings.desired_frame_quaternion_cost_multiplier = torso_orientation_scaling output_settings.joint_regularization_cost_multiplier = 0.1 output_settings.force_regularization_cost_multiplier = 0.2 output_settings.com_regularization_cost_multiplier = 10.0 @@ -110,6 +115,12 @@ def get_planner_settings( if constrain_right_foot_position else hippopt.ExpressionType.minimize ) + output_settings.left_hand_frame_name = "l_hand_palm" + output_settings.left_hand_expression_type = left_hand_expression_type + output_settings.left_hand_regularization_cost_multiplier = left_hand_scaling + output_settings.right_hand_frame_name = "r_hand_palm" + output_settings.right_hand_expression_type = right_hand_expression_type + output_settings.right_hand_regularization_cost_multiplier = right_hand_scaling output_settings.casadi_function_options = {} output_settings.casadi_opti_options = {} output_settings.casadi_solver_options = { @@ -130,6 +141,8 @@ def get_references( desired_left_foot_pose: liecasadi.SE3, desired_right_foot_pose: liecasadi.SE3, desired_com_position: np.ndarray, + desired_left_hand_position: np.ndarray, + desired_right_hand_position: np.ndarray, planner_settings: pose_finder.Settings, ) -> pose_finder.References: output_references = pose_finder.References( @@ -182,6 +195,8 @@ def get_references( -1.52, ] ) + output_references.left_hand_position = desired_left_hand_position + output_references.right_hand_position = desired_right_hand_position return output_references @@ -242,6 +257,13 @@ def complex_pose( force_regularization_cost_multiplier=None, constrain_left_foot_position=False, constrain_right_foot_position=False, + torso_orientation_scaling: float = 100.0, + left_hand_expression_type: hippopt.ExpressionType = hippopt.ExpressionType.skip, + left_hand_scaling: float = 1.0, + desired_left_hand_position: np.ndarray = np.zeros(3), + right_hand_expression_type: hippopt.ExpressionType = hippopt.ExpressionType.skip, + right_hand_scaling: float = 1.0, + desired_right_hand_position: np.ndarray = np.zeros(3), ) -> hippopt.Output[pose_finder.Variables]: terrain = hp_rp.SmoothTerrain.step( length=0.5, @@ -255,6 +277,11 @@ def complex_pose( use_joint_limits=use_joint_limits, constrain_left_foot_position=constrain_left_foot_position, constrain_right_foot_position=constrain_right_foot_position, + torso_orientation_scaling=torso_orientation_scaling, + left_hand_expression_type=left_hand_expression_type, + left_hand_scaling=left_hand_scaling, + right_hand_expression_type=right_hand_expression_type, + right_hand_scaling=right_hand_scaling, ) if casadi_solver_options is not None: planner_settings.casadi_solver_options.update(casadi_solver_options) @@ -275,6 +302,8 @@ def complex_pose( desired_right_foot_position, liecasadi.SO3.Identity() ), desired_com_position=desired_com_position, + desired_left_hand_position=desired_left_hand_position, + desired_right_hand_position=desired_right_hand_position, planner_settings=planner_settings, ) planner.set_references(references) @@ -289,6 +318,8 @@ def complex_pose( if __name__ == "__main__": + complex_poses = {"joint_name_list": joint_names} + # Large step-up 20cm centered step_length = 0.45 @@ -301,7 +332,7 @@ def complex_pose( desired_right_foot_position=np.array([step_length, -0.1, step_height]), desired_com_position=np.array([step_length / 2, 0.0, 0.7]), ) - complex_poses = {"high_step_20cm": output.values.state.to_dict(flatten=False)} + complex_poses["high_step_20cm"] = output.values.state.to_dict(flatten=False) print_ankle_bounds_multipliers( input_solution=output, tag="up20", joint_name_list=joint_names ) @@ -529,6 +560,31 @@ def complex_pose( flatten=False ) + print("Press [Enter] to move to next pose.") + input() + + # Get something from ground + + output = complex_pose( + terrain_height=0.0, + terrain_origin=np.array([0.0, 0.0, 0.0]), + use_joint_limits=True, + desired_left_foot_position=np.array([0.0, 0.1, 0.0]), + desired_right_foot_position=np.array([0.0, -0.1, 0.0]), + desired_com_position=np.array([0.00, 0.0, 0.4]), + casadi_solver_options={"alpha_for_y": "primal"}, + constrain_left_foot_position=True, + constrain_right_foot_position=True, + left_hand_expression_type=hippopt.ExpressionType.subject_to, + right_hand_expression_type=hippopt.ExpressionType.subject_to, + desired_left_hand_position=np.array([0.2, 0.1, 0.2]), + desired_right_hand_position=np.array([0.2, -0.1, 0.2]), + ) + complex_poses["both_hands_down"] = output.values.state.to_dict(flatten=False) + print_ankle_bounds_multipliers( + input_solution=output, tag="bothHandsDown", joint_name_list=joint_names + ) + hdf5storage.savemat( file_name="complex_poses.mat", mdict=complex_poses,