From 6155e6f146a3a498189aa3144dd0a873c815aa37 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 7 Jul 2024 12:48:20 +0200 Subject: [PATCH] Added complex pose with both hands reaching the ground and no limits --- .../main_complex_poses.py | 24 +++++++++++++++++++ 1 file changed, 24 insertions(+) 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 14c8612..4027c02 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 @@ -585,6 +585,30 @@ def complex_pose( input_solution=output, tag="bothHandsDown", joint_name_list=joint_names ) + print("Press [Enter] to move to next pose.") + input() + + # Get something from ground (no limits) + + output = complex_pose( + terrain_height=0.0, + terrain_origin=np.array([0.0, 0.0, 0.0]), + use_joint_limits=False, + 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_no_limit"] = output.values.state.to_dict( + flatten=False + ) + hdf5storage.savemat( file_name="complex_poses.mat", mdict=complex_poses,