Add possibility to consider hand poses in pose finder

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,102 @@ 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
+    )
+
+    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
+    )
+
+    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,

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,