From 8fdb39e836189b3e7f049ba97138403af3e5cccb Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 7 Jul 2024 12:31:21 +0200 Subject: [PATCH 1/4] Added possibility to set desired hand positions in pose finder --- .../humanoid_pose_finder/planner.py | 121 ++++++++++++++++++ 1 file changed, 121 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 9a84ce4..25baf72 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -59,6 +59,15 @@ class Settings: default=None ) + left_hand_frame_name: str = dataclasses.field(default=None) + lef_hand_position_in_frame: np.array = dataclasses.field(default=None) + left_hand_regularization_cost_multiplier: float = dataclasses.field(default=None) + left_hand_expression_type: hp.ExpressionType = dataclasses.field(default=None) + right_hand_frame_name: str = dataclasses.field(default=None) + right_hand_position_in_frame: np.array = dataclasses.field(default=None) + right_hand_regularization_cost_multiplier: float = dataclasses.field(default=None) + right_hand_expression_type: hp.ExpressionType = dataclasses.field(default=None) + opti_solver: str = dataclasses.field(default="ipopt") problem_type: str = dataclasses.field(default="nlp") casadi_function_options: dict = dataclasses.field(default_factory=dict) @@ -74,6 +83,12 @@ def __post_init__(self) -> None: self.com_position_expression_type = hp.ExpressionType.minimize self.left_point_position_expression_type = hp.ExpressionType.minimize self.right_point_position_expression_type = hp.ExpressionType.minimize + self.lef_hand_position_in_frame = np.array([0.0, 0.0, 0.0]) + self.left_hand_regularization_cost_multiplier = 1.0 + self.left_hand_expression_type = hp.ExpressionType.skip + self.right_hand_position_in_frame = np.array([0.0, 0.0, 0.0]) + self.right_hand_regularization_cost_multiplier = 1.0 + self.right_hand_expression_type = hp.ExpressionType.skip def is_valid(self) -> bool: ok = True @@ -144,6 +159,36 @@ def is_valid(self) -> bool: if self.right_point_position_expression_type is None: logger.error("right_point_position_expression_type is None") ok = False + if self.left_hand_expression_type is None: + logger.error("left_hand_expression_type is None") + ok = False + if ( + self.left_hand_expression_type != hp.ExpressionType.skip + and self.left_hand_frame_name is None + ): + logger.error("left_hand_frame_name is None") + ok = False + if self.lef_hand_position_in_frame is None: + logger.error("lef_hand_position_in_frame is None") + ok = False + if self.left_hand_regularization_cost_multiplier is None: + logger.error("left_hand_regularization_cost_multiplier is None") + ok = False + if self.right_hand_expression_type is None: + logger.error("right_hand_expression_type is None") + ok = False + if ( + self.right_hand_expression_type != hp.ExpressionType.skip + and self.right_hand_frame_name is None + ): + logger.error("right_hand_frame_name is None") + ok = False + if self.right_hand_position_in_frame is None: + logger.error("right_hand_position_in_frame is None") + ok = False + if self.right_hand_regularization_cost_multiplier is None: + logger.error("right_hand_regularization_cost_multiplier is None") + ok = False return ok @@ -154,6 +199,8 @@ class References(hp.OptimizationObject): cls=hp.Parameter, factory=hp_rp.HumanoidState ) frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + left_hand_position: hp.StorageType = hp.default_storage_field(hp.Parameter) + right_hand_position: hp.StorageType = hp.default_storage_field(hp.Parameter) contact_point_descriptors: dataclasses.InitVar[ hp_rp.FeetContactPointDescriptors @@ -171,6 +218,8 @@ def __post_init__( ) self.frame_quaternion_xyzw = np.zeros(4) self.frame_quaternion_xyzw[3] = 1.0 + self.left_hand_position = np.zeros(3) + self.right_hand_position = np.zeros(3) @dataclasses.dataclass @@ -195,6 +244,11 @@ class Variables(hp.OptimizationObject): maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + left_hand_position_in_frame: hp.StorageType = hp.default_storage_field(hp.Parameter) + right_hand_position_in_frame: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[adam.casadi.KinDynComputations] = ( dataclasses.field(default=None) @@ -241,6 +295,8 @@ def __post_init__( self.relaxed_complementarity_epsilon = settings.relaxed_complementarity_epsilon self.maximum_joint_positions = settings.maximum_joint_positions self.minimum_joint_positions = settings.minimum_joint_positions + self.left_hand_position_in_frame = settings.lef_hand_position_in_frame + self.right_hand_position_in_frame = settings.right_hand_position_in_frame class Planner: @@ -537,6 +593,71 @@ def _add_kinematics_regularization( scaling=self.settings.joint_regularization_cost_multiplier, ) + if self.settings.left_hand_expression_type != hp.ExpressionType.skip: + left_hand_position_fun = hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=self.settings.left_hand_frame_name, + **function_inputs, + ) + if self.parametric_model: + left_hand_position = left_hand_position_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + pi_l=variables.parametric_link_length_multipliers, + pi_d=variables.parametric_link_densities, + p_parent=variables.left_hand_position_in_frame, + )["point_position"] + else: + left_hand_position = left_hand_position_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + p_parent=variables.left_hand_position_in_frame, + )["point_position"] + + problem.add_expression( + expression=cs.MX( + left_hand_position == variables.references.left_hand_position + ), + name="left_hand_position_error", + scaling=self.settings.left_hand_regularization_cost_multiplier, + mode=self.settings.left_hand_expression_type, + ) + + if self.settings.right_hand_expression_type != hp.ExpressionType.skip: + right_hand_position_fun = hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=self.settings.right_hand_frame_name, + **function_inputs, + ) + + if self.parametric_model: + right_hand_position = right_hand_position_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + pi_l=variables.parametric_link_length_multipliers, + pi_d=variables.parametric_link_densities, + p_parent=variables.right_hand_position_in_frame, + )["point_position"] + else: + right_hand_position = right_hand_position_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + p_parent=variables.right_hand_position_in_frame, + )["point_position"] + + problem.add_expression( + expression=cs.MX( + right_hand_position == variables.references.right_hand_position + ), + name="right_hand_position_error", + scaling=self.settings.right_hand_regularization_cost_multiplier, + mode=self.settings.right_hand_expression_type, + ) + def _add_contact_kinematic_consistency( self, function_inputs: dict, From 277b617fc58209cf8031f491eae4fa319b549438 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 7 Jul 2024 12:31:44 +0200 Subject: [PATCH 2/4] 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, From 8096aa51fc784186e4d09d4acb2f3c9f1fcba0b7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 7 Jul 2024 12:48:20 +0200 Subject: [PATCH 3/4] 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, From 7ce29659a61c039a4334d1972f67f5bef4136570 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 9 Jul 2024 10:40:01 +0200 Subject: [PATCH 4/4] Added poses with hand on the side --- .../main_complex_poses.py | 47 +++++++++++++++++++ 1 file changed, 47 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 4027c02..9d1551c 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 @@ -609,6 +609,53 @@ def complex_pose( flatten=False ) + print("Press [Enter] to move to next pose.") + input() + + # Get something from ground on the side + + 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.skip, + desired_left_hand_position=np.array([0.0, 0.3, 0.1]), + torso_orientation_scaling=1e8, + ) + complex_poses["hand_side"] = output.values.state.to_dict(flatten=False) + print_ankle_bounds_multipliers( + input_solution=output, tag="hand_side", joint_name_list=joint_names + ) + + print("Press [Enter] to move to next pose.") + input() + + # Get something from ground on the side (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": "full"}, + constrain_left_foot_position=True, + constrain_right_foot_position=True, + left_hand_expression_type=hippopt.ExpressionType.subject_to, + right_hand_expression_type=hippopt.ExpressionType.skip, + desired_left_hand_position=np.array([0.0, 0.3, 0.1]), + torso_orientation_scaling=1e8, + ) + complex_poses["hand_side_no_limits"] = output.values.state.to_dict(flatten=False) + hdf5storage.savemat( file_name="complex_poses.mat", mdict=complex_poses,