diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py index 47d820ff..6d7c31dd 100644 --- a/src/hippopt/base/opti_solver.py +++ b/src/hippopt/base/opti_solver.py @@ -152,6 +152,9 @@ def _generate_opti_object( if isinstance(value, float): value = value * np.ones((1, 1)) + if value.shape[0] * value.shape[1] == 0: + raise ValueError("Field " + name + " has a zero dimension.") + if storage_type is Variable.StorageTypeValue: self._logger.debug("Creating variable " + name) opti_object = self._solver.variable(*value.shape) @@ -181,12 +184,17 @@ def _generate_objects_from_instance( composite_value = output.__getattribute__(field.name) is_list = isinstance(composite_value, list) - list_of_optimization_objects = is_list and all( - isinstance(elem, OptimizationObject) or isinstance(elem, list) - for elem in composite_value + list_of_optimization_objects = ( + is_list + and len(composite_value) > 0 + and all( + isinstance(elem, OptimizationObject) or isinstance(elem, list) + for elem in composite_value + ) ) - list_of_float = is_list and all( - isinstance(elem, float) for elem in composite_value + list_of_float = is_list and ( + len(composite_value) == 0 + or all(isinstance(elem, float) for elem in composite_value) ) if list_of_float: composite_value = np.array(composite_value) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index bae777a4..c253dab6 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -1,6 +1,7 @@ import casadi as cs import liecasadi from adam.casadi import KinDynComputations +from adam.parametric.casadi import KinDynComputationsParametric from hippopt.robot_planning.expressions.quaternion import ( quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, @@ -8,13 +9,15 @@ def centroidal_momentum_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", base_position_derivative_name: str = "base_position_derivative", base_quaternion_xyzw_derivative_name: str = "base_quaternion_derivative", joint_velocities_name: str = "joint_velocities", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -27,6 +30,20 @@ def centroidal_momentum_from_kinematics( base_quaternion_derivative = cs.MX.sym(base_quaternion_xyzw_derivative_name, 4) joint_velocities = cs.MX.sym(joint_velocities_name, kindyn_object.NDoF) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + cmm_function = kindyn_object.centroidal_momentum_matrix_fun() base_pose = liecasadi.SE3.from_position_quaternion( @@ -51,6 +68,43 @@ def centroidal_momentum_from_kinematics( base_position_derivative, base_angular_velocity, joint_velocities ) + if parametric: + momentum = ( + cmm_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + @ robot_velocity + ) + return cs.Function( + "centroidal_momentum_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + base_position_derivative, + base_quaternion_derivative, + joint_velocities, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [momentum], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + base_position_derivative_name, + base_quaternion_xyzw_derivative_name, + joint_velocities_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["h_g"], + options, + ) + momentum = cmm_function(base_pose, joint_positions) @ robot_velocity return cs.Function( @@ -78,10 +132,12 @@ def centroidal_momentum_from_kinematics( def center_of_mass_position_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -91,12 +147,53 @@ def center_of_mass_position_from_kinematics( base_quaternion = cs.MX.sym(base_quaternion_xyzw_name, 4) joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + com_function = kindyn_object.CoM_position_fun() base_pose = liecasadi.SE3.from_position_quaternion( base_position, base_quaternion ).as_matrix() # The quaternion is supposed normalized + if parametric: + com_position = com_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + return cs.Function( + "center_of_mass_position_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [com_position], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["com_position"], + options, + ) + com_position = com_function(base_pose, joint_positions) return cs.Function( @@ -118,12 +215,14 @@ def center_of_mass_position_from_kinematics( def point_position_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, frame_name: str, point_position_in_frame_name: str = "point_position", base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -134,16 +233,61 @@ def point_position_from_kinematics( joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) point_position_in_frame = cs.MX.sym(point_position_in_frame_name, 3) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + "parametric_links_length_multiplier", + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + "parametric_links_densities", kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + fk_function = kindyn_object.forward_kinematics_fun(frame=frame_name) base_pose = liecasadi.SE3.from_position_quaternion( base_position, base_quaternion ).as_matrix() # The quaternion is supposed normalized - frame_pose = fk_function(base_pose, joint_positions) + if parametric: + frame_pose = fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + else: + frame_pose = fk_function(base_pose, joint_positions) point_position = frame_pose[:3, :3] @ point_position_in_frame + frame_pose[:3, 3] + if parametric: + return cs.Function( + "point_position_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + point_position_in_frame, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [point_position], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + point_position_in_frame_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["point_position"], + options, + ) + return cs.Function( "point_position_from_kinematics", [ @@ -165,34 +309,81 @@ def point_position_from_kinematics( def frames_relative_position( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, reference_frame: str, target_frame: str, joint_positions_name: str = "joint_positions", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: options = {} if options is None else options joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + base_pose = cs.DM_eye(4) reference_fk_function = kindyn_object.forward_kinematics_fun(frame=reference_frame) target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) - reference_pose = reference_fk_function(base_pose, joint_positions) + if parametric: + reference_pose = reference_fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + target_pose = target_fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + else: + reference_pose = reference_fk_function(base_pose, joint_positions) + target_pose = target_fk_function(base_pose, joint_positions) + reference_pose_inverse_rotation = reference_pose[:3, :3].T reference_pose_inverse_translation = ( -reference_pose_inverse_rotation @ reference_pose[:3, 3] ) - target_pose = target_fk_function(base_pose, joint_positions) - relative_position = ( reference_pose_inverse_rotation @ target_pose[:3, 3] + reference_pose_inverse_translation ) + if parametric: + return cs.Function( + "frames_relative_position_parametric", + [ + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [relative_position], + [ + joint_positions_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["relative_position"], + options, + ) + return cs.Function( "frames_relative_position", [joint_positions], @@ -204,12 +395,14 @@ def frames_relative_position( def rotation_error_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, target_frame: str, base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", desired_quaternion_xyzw_name: str = "desired_quaternion", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -219,18 +412,65 @@ def rotation_error_from_kinematics( joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) desired_quaternion = cs.MX.sym(desired_quaternion_xyzw_name, 4) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + base_pose = liecasadi.SE3.from_position_quaternion( base_position, base_quaternion ).as_matrix() # The quaternion is supposed normalized target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) - target_pose = target_fk_function(base_pose, joint_positions) + + if parametric: + target_pose = target_fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + else: + target_pose = target_fk_function(base_pose, joint_positions) + target_orientation = target_pose[:3, :3] rotation_error = ( target_orientation @ liecasadi.SO3.from_quat(desired_quaternion).as_matrix().T ) + if parametric: + return cs.Function( + "rotation_error_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + desired_quaternion, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [rotation_error], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + desired_quaternion_xyzw_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["rotation_error"], + options, + ) + return cs.Function( "quaternion_error_from_kinematics", [ diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index f5086878..05f136bb 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -4,9 +4,11 @@ import pathlib import time +import adam.model import ffmpeg import liecasadi import numpy as np +from adam.model.conversions import to_idyntree_model from idyntree.visualize import MeshcatVisualizer from hippopt.robot_planning.utilities.terrain_visualizer import ( @@ -21,7 +23,7 @@ @dataclasses.dataclass class HumanoidStateVisualizerSettings(TerrainVisualizerSettings): - robot_model: str = dataclasses.field(default=None) + robot_model: str | adam.model.Model = dataclasses.field(default=None) considered_joints: list[str] = dataclasses.field(default=None) contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) robot_color: list[float] = dataclasses.field(default=None) @@ -122,12 +124,23 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: self._set_clone_visibility(i, False) def _allocate_clone(self, index: int) -> None: - self._viz.load_model_from_file( - model_path=self._settings.robot_model, - model_name=f"[{index}]robot", - considered_joints=self._settings.considered_joints, - color=self._settings.robot_color, - ) + + if isinstance(self._settings.robot_model, str): + self._viz.load_model_from_file( + model_path=self._settings.robot_model, + model_name=f"[{index}]robot", + considered_joints=self._settings.considered_joints, + color=self._settings.robot_color, + ) + else: + idyntree_model = adam.model.conversions.to_idyntree_model( + self._settings.robot_model + ) + self._viz.load_model( + model=idyntree_model, + model_name=f"[{index}]robot", + color=self._settings.robot_color, + ) self._viz.load_sphere( radius=self._settings.com_radius, shape_name=f"[{index}]CoM", diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py new file mode 100644 index 00000000..1a69d74d --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py @@ -0,0 +1,561 @@ +import logging + +import casadi as cs +import idyntree.bindings as idyntree +import liecasadi +import numpy as np +import resolve_robotics_uri_py + +import hippopt +import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_kinodynamic.planner as walking_planner +import hippopt.turnkey_planners.humanoid_kinodynamic.settings as walking_settings +import hippopt.turnkey_planners.humanoid_kinodynamic.variables as walking_variables +import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder + + +def get_planner_settings() -> walking_settings.Settings: + # The model is available at + # https://github.com/icub-tech-iit/ergocub-gazebo-simulations/tree/↵ + # 1179630a88541479df51ebb108a21865ea251302/models/stickBot + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://stickBot/model.urdf" + ) + settings = walking_settings.Settings() + settings.robot_urdf = str(urdf_path) + settings.joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", + ] + + number_of_joints = len(settings.joints_name_list) + idyntree_model_loader = idyntree.ModelLoader() + idyntree_model_loader.loadReducedModelFromFile( + settings.robot_urdf, settings.joints_name_list + ) + idyntree_model = idyntree_model_loader.model() + settings.root_link = "root_link" + settings.horizon_length = 30 + settings.time_step = 0.1 + settings.contact_points = hp_rp.FeetContactPointDescriptors() + settings.contact_points.left = hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="l_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.05, 0.0]), + ) + settings.contact_points.right = hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="r_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.05, 0.0]), + ) + settings.planar_dcc_height_multiplier = 10.0 + settings.dcc_gain = 40.0 + settings.dcc_epsilon = 0.005 + settings.static_friction = 0.3 + settings.maximum_velocity_control = [2.0, 2.0, 5.0] + settings.maximum_force_derivative = [500.0, 500.0, 500.0] + settings.maximum_angular_momentum = 5.0 + settings.minimum_com_height = 0.3 + settings.minimum_feet_lateral_distance = 0.1 + settings.maximum_feet_relative_height = 0.05 + settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) + settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) + for i in range(number_of_joints): + joint = idyntree_model.getJoint(i) + if joint.hasPosLimits(): + settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) + settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) + settings.maximum_joint_velocities = np.ones(number_of_joints) * 2.0 + settings.minimum_joint_velocities = np.ones(number_of_joints) * -2.0 + settings.joint_regularization_cost_weights = np.ones(number_of_joints) + settings.joint_regularization_cost_weights[:3] = 0.1 # torso + settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + settings.joint_regularization_cost_weights[11:] = 1.0 # legs + settings.contacts_centroid_cost_multiplier = 0.0 + settings.com_linear_velocity_cost_weights = [10.0, 0.1, 1.0] + settings.com_linear_velocity_cost_multiplier = 1.0 + settings.desired_frame_quaternion_cost_frame_name = "chest" + settings.desired_frame_quaternion_cost_multiplier = 200.0 + settings.base_quaternion_cost_multiplier = 50.0 + settings.base_quaternion_velocity_cost_multiplier = 0.001 + settings.joint_regularization_cost_multiplier = 10.0 + settings.force_regularization_cost_multiplier = 10.0 + settings.foot_yaw_regularization_cost_multiplier = 2000.0 + settings.swing_foot_height_cost_multiplier = 1000.0 + settings.contact_velocity_control_cost_multiplier = 5.0 + settings.contact_force_control_cost_multiplier = 0.0001 + 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_solver_options = { + "max_iter": 4000, + "linear_solver": "mumps", + "alpha_for_y": "dual-and-full", + "fast_step_computation": "yes", + "hessian_approximation": "limited-memory", + "tol": 1e-3, + "dual_inf_tol": 1000.0, + "compl_inf_tol": 1e-2, + "constr_viol_tol": 1e-4, + "acceptable_tol": 10, + "acceptable_iter": 2, + "acceptable_compl_inf_tol": 1000.0, + "warm_start_bound_frac": 1e-2, + "warm_start_bound_push": 1e-2, + "warm_start_mult_bound_push": 1e-2, + "warm_start_slack_bound_frac": 1e-2, + "warm_start_slack_bound_push": 1e-2, + "warm_start_init_point": "yes", + "required_infeasibility_reduction": 0.8, + "perturb_dec_fact": 0.1, + "max_hessian_perturbation": 100.0, + "acceptable_obj_change_tol": 1e0, + } + + return settings + + +def get_pose_finder_settings( + input_settings: walking_settings.Settings, +) -> pose_finder.Settings: + number_of_joints = len(input_settings.joints_name_list) + settings = pose_finder.Settings() + settings.robot_urdf = input_settings.robot_urdf + settings.joints_name_list = input_settings.joints_name_list + settings.parametric_link_names = input_settings.parametric_link_names + + settings.root_link = input_settings.root_link + settings.desired_frame_quaternion_cost_frame_name = ( + input_settings.desired_frame_quaternion_cost_frame_name + ) + + settings.contact_points = input_settings.contact_points + + settings.relaxed_complementarity_epsilon = 0.0001 + settings.static_friction = input_settings.static_friction + + settings.maximum_joint_positions = input_settings.maximum_joint_positions + settings.minimum_joint_positions = input_settings.minimum_joint_positions + + settings.joint_regularization_cost_weights = np.ones(number_of_joints) + settings.joint_regularization_cost_weights[:3] = 0.1 # torso + settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + settings.joint_regularization_cost_weights[11:] = 1.0 # legs + + settings.base_quaternion_cost_multiplier = 50.0 + settings.desired_frame_quaternion_cost_multiplier = 100.0 + settings.joint_regularization_cost_multiplier = 0.1 + settings.force_regularization_cost_multiplier = 0.2 + settings.com_regularization_cost_multiplier = 10.0 + settings.average_force_regularization_cost_multiplier = 10.0 + settings.point_position_regularization_cost_multiplier = 100.0 + settings.casadi_function_options = input_settings.casadi_function_options + settings.casadi_opti_options = input_settings.casadi_opti_options + settings.casadi_solver_options = {} + + return settings + + +def get_visualizer_settings( + input_settings: walking_settings.Settings, +) -> hp_rp.HumanoidStateVisualizerSettings: + output_viz_settings = hp_rp.HumanoidStateVisualizerSettings() + output_viz_settings.robot_model = input_settings.robot_urdf + output_viz_settings.considered_joints = input_settings.joints_name_list + output_viz_settings.contact_points = input_settings.contact_points + output_viz_settings.terrain = input_settings.terrain + output_viz_settings.working_folder = "./" + + return output_viz_settings + + +def compute_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + desired_com_position: np.ndarray, + desired_left_foot_pose: liecasadi.SE3, + desired_right_foot_pose: liecasadi.SE3, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, +) -> hp_rp.HumanoidState: + desired_joints = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + assert len(input_settings.joints_name_list) == len(desired_joints) + + pf_ref = pose_finder.References( + contact_point_descriptors=pf_settings.contact_points, + number_of_joints=len(desired_joints), + ) + + pf_ref.state.com = desired_com_position + pf_ref.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + pf_ref.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + pf_ref.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + + pf_ref.state.kinematics.joints.positions = desired_joints + + pf_guess = pf_input.get_initial_guess() + pf_guess.references = pf_ref + if pf_parametric_link_densities is not None: + pf_guess.parametric_link_densities = pf_parametric_link_densities + if pf_parametric_link_length_multipliers is not None: + pf_guess.parametric_link_length_multipliers = ( + pf_parametric_link_length_multipliers + ) + pf_input.set_initial_guess(pf_guess) + + output_pf = pf_input.solve() + return output_pf.values.state + + +def compute_initial_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, +) -> walking_variables.ExtendedHumanoidState: + desired_left_foot_pose = contact_guess.left[0].transform + desired_right_foot_pose = contact_guess.right[0].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] = 0.7 + output_pf = compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + pf_parametric_link_densities=pf_parametric_link_densities, + pf_parametric_link_length_multipliers=pf_parametric_link_length_multipliers, + ) + + output_state = walking_variables.ExtendedHumanoidState() + output_state.contact_points = output_pf.contact_points + output_state.kinematics = output_pf.kinematics + output_state.com = output_pf.com + + output_state.centroidal_momentum = np.zeros((6, 1)) + + return output_state + + +def compute_middle_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[1].transform + desired_right_foot_pose = contact_guess.right[0].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] = 0.7 + return compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + pf_parametric_link_densities=pf_parametric_link_densities, + pf_parametric_link_length_multipliers=pf_parametric_link_length_multipliers, + ) + + +def compute_final_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[1].transform + desired_right_foot_pose = contact_guess.right[1].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] = 0.7 + return compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + pf_parametric_link_densities=pf_parametric_link_densities, + pf_parametric_link_length_multipliers=pf_parametric_link_length_multipliers, + ) + + +def get_references( + input_settings: walking_settings.Settings, + desired_states: list[hp_rp.HumanoidState], +) -> list[walking_variables.References]: + output_list = [] + + for i in range(input_settings.horizon_length): + output_reference = walking_variables.References( + number_of_joints=len(input_settings.joints_name_list), + number_of_points_left=len(input_settings.contact_points.left), + number_of_points_right=len(input_settings.contact_points.right), + ) + + output_reference.contacts_centroid_cost_weights = [100, 100, 10] + output_reference.contacts_centroid = [0.3, 0.0, 0.0] + output_reference.joint_regularization = desired_states[ + i + ].kinematics.joints.positions + output_reference.com_linear_velocity = [0.1, 0.0, 0.0] + output_list.append(output_reference) + + return output_list + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + + planner_settings = get_planner_settings() + + planner_settings.parametric_link_names = [ + "r_upper_arm", + "r_forearm", + "l_hip_3", + "l_lower_leg", + "root_link", + "torso_1", + "torso_2", + "chest", + ] + + parametric_link_densities = [ + 1578.8230690646876, + 687.9855671524874, + 568.2817642184916, + 1907.2410446310623, + 2013.8319822728106, + 1134.0550335996697, + 844.6779189491116, + 628.0724496264946, + ] + parametric_link_length_multipliers = [ + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 2.0, + ] + + planner = walking_planner.Planner(settings=planner_settings) + + pf_settings = get_pose_finder_settings(input_settings=planner_settings) + pf = pose_finder.Planner(settings=pf_settings) + + horizon = planner_settings.horizon_length * planner_settings.time_step + + step_length = 0.6 + + contact_phases_guess = hp_rp.FeetContactPhasesDescriptor() + contact_phases_guess.left = [ + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length / 2, 0.1, 0.05]), liecasadi.SO3.Identity() + ), + force=np.array([0, 0, 100.0]), + activation_time=None, + deactivation_time=horizon / 6.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length, 0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=None, + force=np.array([0, 0, 100.0]), + activation_time=horizon / 3.0, + deactivation_time=None, + ), + ] + + contact_phases_guess.right = [ + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length / 2, -0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length, -0.1, 0.05]), liecasadi.SO3.Identity() + ), + force=np.array([0, 0, 100.0]), + activation_time=None, + deactivation_time=horizon * 2.0 / 3.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([1.5 * step_length, -0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=None, + force=np.array([0, 0, 100.0]), + activation_time=horizon * 5.0 / 6.0, + deactivation_time=None, + ), + ] + + initial_state = compute_initial_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + pf_parametric_link_densities=parametric_link_densities, + pf_parametric_link_length_multipliers=parametric_link_length_multipliers, + ) + + final_state = compute_final_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + pf_parametric_link_densities=parametric_link_densities, + pf_parametric_link_length_multipliers=parametric_link_length_multipliers, + ) + final_state.centroidal_momentum = np.zeros((6, 1)) + + middle_state = compute_middle_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + pf_parametric_link_densities=parametric_link_densities, + pf_parametric_link_length_multipliers=parametric_link_length_multipliers, + ) + + first_half_guess_length = planner_settings.horizon_length // 2 + first_half_guess = hp_rp.humanoid_state_interpolator( + initial_state=initial_state, + final_state=middle_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=first_half_guess_length, + dt=planner_settings.time_step, + ) + + second_half_guess_length = planner_settings.horizon_length - first_half_guess_length + second_half_guess = hp_rp.humanoid_state_interpolator( + initial_state=middle_state, + final_state=final_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=second_half_guess_length, + dt=planner_settings.time_step, + t0=first_half_guess_length * planner_settings.time_step, + ) + + guess = first_half_guess + second_half_guess + + print("Starting the planner...") + + references = get_references( + input_settings=planner_settings, + desired_states=guess, + ) + + planner.set_references(references) + planner_guess = planner.get_initial_guess() + planner_guess.system = [ + walking_variables.ExtendedHumanoid.from_humanoid_state(s) for s in guess + ] + planner_guess.initial_state = initial_state + planner_guess.final_state = final_state + if parametric_link_densities is not None: + planner_guess.parametric_link_densities = parametric_link_densities + if parametric_link_length_multipliers is not None: + planner_guess.parametric_link_length_multipliers = ( + parametric_link_length_multipliers + ) + planner.set_initial_guess(planner_guess) + + output = planner.solve() + + humanoid_states = [s.to_humanoid_state() for s in output.values.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) + 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.values.dt, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_periodic_parametric", + ) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index d72db04e..d1f2aeb5 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -1,8 +1,14 @@ import copy import adam.casadi +import adam.model +import adam.numpy +import adam.parametric.casadi import casadi as cs import numpy as np +from adam.parametric.model.parametric_factories.parametric_model import ( + URDFParametricModelFactory, +) import hippopt as hp import hippopt.robot_planning as hp_rp @@ -21,14 +27,26 @@ def __init__(self, settings: Settings) -> None: if not settings.is_valid(): raise ValueError("Settings are not valid") self.settings = copy.deepcopy(settings) - self.kin_dyn_object = adam.casadi.KinDynComputations( - urdfstring=self.settings.robot_urdf, - joints_name_list=self.settings.joints_name_list, - root_link=self.settings.root_link, - gravity=self.settings.gravity, - f_opts=self.settings.casadi_function_options, - ) - self.numeric_mass = self.kin_dyn_object.get_total_mass() + if self.settings.parametric_link_names is not None: + self.parametric_model = True + self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + links_name_list=self.settings.parametric_link_names, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + else: + self.parametric_model = False + self.kin_dyn_object = adam.casadi.KinDynComputations( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + self.numeric_mass = self.kin_dyn_object.get_total_mass() self.variables = Variables( settings=self.settings, kin_dyn_object=self.kin_dyn_object @@ -184,6 +202,8 @@ def _get_function_inputs_dict(self): "second_point_name": "p_1", "desired_yaw_name": "yd", "desired_height_name": "hd", + "parametric_link_length_multipliers_name": "pi_l", + "parametric_link_densities_name": "pi_d", "options": self.settings.casadi_function_options, } return function_inputs @@ -261,11 +281,20 @@ def _add_kinematics_constraints( com_kinematics_fun = hp_rp.center_of_mass_position_from_kinematics( kindyn_object=self.kin_dyn_object, **function_inputs ) - com_kinematics = com_kinematics_fun( - pb=sym.system.kinematics.base.position, - qb=normalized_quaternion, - s=sym.system.kinematics.joints.positions, - )["com_position"] + if self.parametric_model: + com_kinematics = com_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + )["com_position"] + else: + com_kinematics = com_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + )["com_position"] problem.add_expression_to_horizon( expression=cs.MX(sym.system.com == com_kinematics), apply_to_first_elements=False, @@ -276,14 +305,26 @@ def _add_kinematics_constraints( centroidal_kinematics_fun = hp_rp.centroidal_momentum_from_kinematics( kindyn_object=self.kin_dyn_object, **function_inputs ) - centroidal_kinematics = centroidal_kinematics_fun( - pb=sym.system.kinematics.base.position, - qb=normalized_quaternion, - s=sym.system.kinematics.joints.positions, - pb_dot=sym.system.kinematics.base.linear_velocity, - qb_dot=sym.system.kinematics.base.quaternion_velocity_xyzw, - s_dot=sym.system.kinematics.joints.velocities, - )["h_g"] + if self.parametric_model: + centroidal_kinematics = centroidal_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + pb_dot=sym.system.kinematics.base.linear_velocity, + qb_dot=sym.system.kinematics.base.quaternion_velocity_xyzw, + s_dot=sym.system.kinematics.joints.velocities, + )["h_g"] + else: + centroidal_kinematics = centroidal_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pb_dot=sym.system.kinematics.base.linear_velocity, + qb_dot=sym.system.kinematics.base.quaternion_velocity_xyzw, + s_dot=sym.system.kinematics.joints.velocities, + )["h_g"] problem.add_expression_to_horizon( expression=cs.MX( sym.system.centroidal_momentum[3:] @@ -321,9 +362,16 @@ def _add_kinematics_constraints( target_frame=left_frame, **function_inputs, ) - relative_position = relative_position_fun( - s=sym.system.kinematics.joints.positions - )["relative_position"] + if self.parametric_model: + relative_position = relative_position_fun( + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + )["relative_position"] + else: + relative_position = relative_position_fun( + s=sym.system.kinematics.joints.positions + )["relative_position"] problem.add_expression_to_horizon( expression=cs.MX(relative_position[1] >= sym.minimum_feet_lateral_distance), apply_to_first_elements=False, @@ -400,12 +448,22 @@ def _add_kinematics_regularization( target_frame=self.settings.desired_frame_quaternion_cost_frame_name, **function_inputs, ) - rotation_error_kinematics = rotation_error_kinematics_fun( - pb=sym.system.kinematics.base.position, - qb=base_quaternion_normalized, - s=sym.system.kinematics.joints.positions, - qd=sym.references.desired_frame_quaternion_xyzw, - )["rotation_error"] + if self.parametric_model: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=base_quaternion_normalized, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + qd=sym.references.desired_frame_quaternion_xyzw, + )["rotation_error"] + else: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=base_quaternion_normalized, + s=sym.system.kinematics.joints.positions, + qd=sym.references.desired_frame_quaternion_xyzw, + )["rotation_error"] problem.add_expression_to_horizon( expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), apply_to_first_elements=False, @@ -547,12 +605,22 @@ def _add_contact_kinematic_consistency( ) # Consistency between the contact position and the kinematics - point_kinematics = point_kinematics_functions[descriptor.foot_frame]( - pb=sym.system.kinematics.base.position, - qb=normalized_quaternion, - s=sym.system.kinematics.joints.positions, - p_parent=descriptor.position_in_foot_frame, - )["point_position"] + if self.parametric_model: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] + else: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] problem.add_expression_to_horizon( expression=cs.MX(point.p == point_kinematics), apply_to_first_elements=False, @@ -858,7 +926,19 @@ def _add_periodicity_expression(self, all_contact_points): ) def _apply_mass_regularization(self, input_var: Variables) -> Variables: - assert self.numeric_mass > 0 + if self.parametric_model: + assert isinstance( + self.kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ) + numeric_mass_fun = self.kin_dyn_object.get_total_mass() + numeric_mass = numeric_mass_fun( + input_var.parametric_link_length_multipliers, + input_var.parametric_link_densities, + ) + else: + numeric_mass = self.numeric_mass + + assert numeric_mass > 0 output = input_var if output.initial_state is not None: if ( @@ -866,19 +946,19 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: and len(output.initial_state.centroidal_momentum.shape) > 0 and output.initial_state.centroidal_momentum.shape[0] == 6 ): - output.initial_state.centroidal_momentum /= self.numeric_mass + output.initial_state.centroidal_momentum /= numeric_mass for point in ( output.initial_state.contact_points.left + output.initial_state.contact_points.right ): - point.f /= self.numeric_mass + point.f /= numeric_mass if output.final_state is not None: for point in ( output.final_state.contact_points.left + output.final_state.contact_points.right ): - point.f /= self.numeric_mass + point.f /= numeric_mass if output.system is None: return output @@ -889,14 +969,26 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: for system in system_list: if system.centroidal_momentum is not None: - system.centroidal_momentum /= self.numeric_mass + system.centroidal_momentum /= numeric_mass for point in system.contact_points.left + system.contact_points.right: - point.f /= self.numeric_mass + point.f /= numeric_mass return output def _undo_mass_regularization(self, input_var: Variables) -> Variables: - assert self.numeric_mass > 0 + if self.parametric_model: + assert isinstance( + self.kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ) + numeric_mass_fun = self.kin_dyn_object.get_total_mass() + numeric_mass = numeric_mass_fun( + input_var.parametric_link_length_multipliers, + input_var.parametric_link_densities, + ) + else: + numeric_mass = self.numeric_mass + + assert numeric_mass > 0 output = input_var if output.initial_state is not None: if ( @@ -904,19 +996,19 @@ def _undo_mass_regularization(self, input_var: Variables) -> Variables: and len(output.initial_state.centroidal_momentum.shape) > 0 and output.initial_state.centroidal_momentum.shape[0] == 6 ): - output.initial_state.centroidal_momentum *= self.numeric_mass + output.initial_state.centroidal_momentum *= numeric_mass for point in ( output.initial_state.contact_points.left + output.initial_state.contact_points.right ): - point.f *= self.numeric_mass + point.f *= numeric_mass if output.final_state is not None: for point in ( output.final_state.contact_points.left + output.final_state.contact_points.right ): - point.f *= self.numeric_mass + point.f *= numeric_mass if output.system is None: return output @@ -927,9 +1019,9 @@ def _undo_mass_regularization(self, input_var: Variables) -> Variables: for system in system_list: if system.centroidal_momentum is not None: - system.centroidal_momentum *= self.numeric_mass + system.centroidal_momentum *= numeric_mass for point in system.contact_points.left + system.contact_points.right: - point.f *= self.numeric_mass + point.f *= numeric_mass return output @@ -970,3 +1062,22 @@ def solve(self) -> hp.Output[Variables]: output = self.ocp.problem.solve() output.values = self._undo_mass_regularization(output.values) return output + + def get_adam_model(self) -> adam.model.Model: + if self.parametric_model: + guess = self.ocp.problem.get_initial_guess() + original_length = guess.parametric_link_length_multipliers + original_density = guess.parametric_link_densities + factory = URDFParametricModelFactory( + path=self.settings.robot_urdf, + math=adam.numpy.numpy_like.SpatialMath(), + links_name_list=self.settings.parametric_link_names, + length_multiplier=original_length, + densities=original_density, + ) + model = adam.model.Model.build( + factory=factory, joints_name_list=self.settings.joints_name_list + ) + return model + + return self.kin_dyn_object.rbdalgos.model diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index 51d15413..d3e44745 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -1,4 +1,5 @@ import dataclasses +import logging import typing import numpy as np @@ -12,6 +13,7 @@ class Settings: robot_urdf: str = dataclasses.field(default=None) joints_name_list: list[str] = dataclasses.field(default=None) + parametric_link_names: list[str] = dataclasses.field(default=None) contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) gravity: np.array = dataclasses.field(default=None) @@ -85,59 +87,180 @@ class Settings: casadi_solver_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: - self.root_link = "root_link" - self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) - self.integrator = hp_int.ImplicitTrapezoid - self.terrain = hp_rp.PlanarTerrain() - self.planar_dcc_height_multiplier = 10.0 - self.dcc_gain = 20.0 - self.dcc_epsilon = 0.05 - self.static_friction = 0.3 - self.maximum_velocity_control = np.array([2.0, 2.0, 5.0]) - self.maximum_force_derivative = np.array([100.0, 100.0, 100.0]) - self.maximum_angular_momentum = 10.0 - self.final_state_expression_type = hp.ExpressionType.skip - self.final_state_expression_weight = 1.0 - self.periodicity_expression_type = hp.ExpressionType.skip - self.periodicity_expression_weight = 1.0 - self.use_opti_callback = False - self.acceptable_constraint_violation = 1e-3 - self.opti_callback_save_costs = True - self.opti_callback_save_constraint_multipliers = True + if self.root_link is None: + self.root_link = "root_link" + + if self.gravity is None: + self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) + + if self.integrator is None: + self.integrator = hp_int.ImplicitTrapezoid + + if self.terrain is None: + self.terrain = hp_rp.PlanarTerrain() + + if self.planar_dcc_height_multiplier is None: + self.planar_dcc_height_multiplier = 10.0 + + if self.dcc_gain is None: + self.dcc_gain = 20.0 + + if self.dcc_epsilon is None: + self.dcc_epsilon = 0.05 + + if self.static_friction is None: + self.static_friction = 0.3 + + if self.maximum_velocity_control is None: + self.maximum_velocity_control = np.array([2.0, 2.0, 5.0]) + + if self.maximum_force_derivative is None: + self.maximum_force_derivative = np.array([100.0, 100.0, 100.0]) + + if self.maximum_angular_momentum is None: + self.maximum_angular_momentum = 10.0 + + if self.final_state_expression_type is None: + self.final_state_expression_type = hp.ExpressionType.skip + + if self.final_state_expression_weight is None: + self.final_state_expression_weight = 1.0 + + if self.periodicity_expression_type is None: + self.periodicity_expression_type = hp.ExpressionType.skip + + if self.periodicity_expression_weight is None: + self.periodicity_expression_weight = 1.0 + + if self.use_opti_callback is None: + self.use_opti_callback = False + + if self.acceptable_constraint_violation is None: + self.acceptable_constraint_violation = 1e-3 + + if self.opti_callback_save_costs is None: + self.opti_callback_save_costs = True + + if self.opti_callback_save_constraint_multipliers is None: + self.opti_callback_save_constraint_multipliers = True def is_valid(self) -> bool: + ok = True + logger = logging.getLogger("[hippopt::HumanoidKynodynamic::Settings]") number_of_joints = len(self.joints_name_list) - return ( - self.robot_urdf is not None - and self.joints_name_list is not None - and self.contact_points is not None - and self.horizon_length is not None - and self.time_step is not None - and self.minimum_com_height is not None - and self.minimum_feet_lateral_distance is not None - and self.maximum_feet_relative_height is not None - and self.maximum_joint_positions is not None - and self.minimum_joint_positions is not None - and self.maximum_joint_velocities is not None - and self.minimum_joint_velocities is not None - and len(self.maximum_joint_positions) == number_of_joints - and len(self.minimum_joint_positions) == number_of_joints - and len(self.maximum_joint_velocities) == number_of_joints - and len(self.minimum_joint_velocities) == number_of_joints - and self.contacts_centroid_cost_multiplier is not None - and self.com_linear_velocity_cost_weights is not None - and len(self.com_linear_velocity_cost_weights) == 3 - and self.com_linear_velocity_cost_multiplier is not None - and self.desired_frame_quaternion_cost_frame_name is not None - and self.desired_frame_quaternion_cost_multiplier is not None - and self.base_quaternion_cost_multiplier is not None - and self.base_quaternion_velocity_cost_multiplier is not None - and self.joint_regularization_cost_weights is not None - and len(self.joint_regularization_cost_weights) == number_of_joints - and self.joint_regularization_cost_multiplier is not None - and self.force_regularization_cost_multiplier is not None - and self.foot_yaw_regularization_cost_multiplier is not None - and self.swing_foot_height_cost_multiplier is not None - and self.contact_velocity_control_cost_multiplier is not None - and self.contact_force_control_cost_multiplier is not None - ) + if self.robot_urdf is None: + logger.error("robot_urdf is None") + ok = False + if self.joints_name_list is None: + logger.error("joints_name_list is None") + ok = False + if self.contact_points is None: + logger.error("contact_points is None") + ok = False + if self.horizon_length is None: + logger.error("horizon_length is None") + ok = False + if self.time_step is None: + logger.error("time_step is None") + ok = False + if self.minimum_com_height is None: + logger.error("minimum_com_height is None") + ok = False + if self.minimum_feet_lateral_distance is None: + logger.error("minimum_feet_lateral_distance is None") + ok = False + if self.maximum_feet_relative_height is None: + logger.error("maximum_feet_relative_height is None") + ok = False + if self.maximum_joint_positions is None: + logger.error("maximum_joint_positions is None") + ok = False + if self.minimum_joint_positions is None: + logger.error("minimum_joint_positions is None") + ok = False + if self.maximum_joint_velocities is None: + logger.error("maximum_joint_velocities is None") + ok = False + if self.minimum_joint_velocities is None: + logger.error("minimum_joint_velocities is None") + ok = False + if len(self.maximum_joint_positions) != number_of_joints: + logger.error( + f"len(maximum_joint_positions)={len(self.maximum_joint_positions)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if len(self.minimum_joint_positions) != number_of_joints: + logger.error( + f"len(minimum_joint_positions)={len(self.minimum_joint_positions)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if len(self.maximum_joint_velocities) != number_of_joints: + logger.error( + f"len(maximum_joint_velocities)={len(self.maximum_joint_velocities)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if len(self.minimum_joint_velocities) != number_of_joints: + logger.error( + f"len(minimum_joint_velocities)={len(self.minimum_joint_velocities)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if self.contacts_centroid_cost_multiplier is None: + logger.error("contacts_centroid_cost_multiplier is None") + ok = False + if self.com_linear_velocity_cost_weights is None: + logger.error("com_linear_velocity_cost_weights is None") + ok = False + if len(self.com_linear_velocity_cost_weights) != 3: + logger.error( + f"len(com_linear_velocity_cost_weights)=" + f"{len(self.com_linear_velocity_cost_weights)} != 3" + ) + ok = False + if self.com_linear_velocity_cost_multiplier is None: + logger.error("com_linear_velocity_cost_multiplier is None") + ok = False + if self.desired_frame_quaternion_cost_frame_name is None: + logger.error("desired_frame_quaternion_cost_frame_name is None") + ok = False + if self.desired_frame_quaternion_cost_multiplier is None: + logger.error("desired_frame_quaternion_cost_multiplier is None") + ok = False + if self.base_quaternion_cost_multiplier is None: + logger.error("base_quaternion_cost_multiplier is None") + ok = False + if self.base_quaternion_velocity_cost_multiplier is None: + logger.error("base_quaternion_velocity_cost_multiplier is None") + ok = False + if self.joint_regularization_cost_weights is None: + logger.error("joint_regularization_cost_weights is None") + ok = False + if len(self.joint_regularization_cost_weights) != number_of_joints: + logger.error( + f"len(joint_regularization_cost_weights)=" + f"{len(self.joint_regularization_cost_weights)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if self.joint_regularization_cost_multiplier is None: + logger.error("joint_regularization_cost_multiplier is None") + ok = False + if self.force_regularization_cost_multiplier is None: + logger.error("force_regularization_cost_multiplier is None") + ok = False + if self.foot_yaw_regularization_cost_multiplier is None: + logger.error("foot_yaw_regularization_cost_multiplier is None") + ok = False + if self.swing_foot_height_cost_multiplier is None: + logger.error("swing_foot_height_cost_multiplier is None") + ok = False + if self.contact_velocity_control_cost_multiplier is None: + logger.error("contact_velocity_control_cost_multiplier is None") + ok = False + if self.contact_force_control_cost_multiplier is None: + logger.error("contact_force_control_cost_multiplier is None") + ok = False + return ok diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index a0978f61..d1aaa5ba 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -2,6 +2,7 @@ from typing import TypeVar import adam.casadi +import adam.parametric.casadi import numpy as np import hippopt as hp @@ -259,6 +260,11 @@ class Variables(hp.OptimizationObject): mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + parametric_link_length_multipliers: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + parametric_link_densities: hp.StorageType = hp.default_storage_field(hp.Parameter) + initial_state: hp.CompositeType[ExtendedHumanoidState] = hp.default_composite_field( cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False ) @@ -295,14 +301,18 @@ class Variables(hp.OptimizationObject): ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) - kin_dyn_object: dataclasses.InitVar[adam.casadi.KinDynComputations] = ( - dataclasses.field(default=None) - ) + kin_dyn_object: dataclasses.InitVar[ + adam.casadi.KinDynComputations + | adam.parametric.casadi.KinDynComputationsParametric + ] = dataclasses.field(default=None) def __post_init__( self, settings: Settings, - kin_dyn_object: adam.casadi.KinDynComputations, + kin_dyn_object: ( + adam.casadi.KinDynComputations + | adam.parametric.casadi.KinDynComputationsParametric + ), ) -> None: self.system = ExtendedHumanoid( contact_point_descriptors=settings.contact_points, @@ -321,7 +331,27 @@ def __post_init__( self.dt = settings.time_step self.gravity = kin_dyn_object.g - self.mass = kin_dyn_object.get_total_mass() + + self.parametric_link_length_multipliers = ( + np.ones(len(settings.parametric_link_names)) + if settings.parametric_link_names is not None + else 0.0 + ) + + if isinstance( + kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ): + self.parametric_link_densities = kin_dyn_object.get_original_densities() + total_mass_fun = kin_dyn_object.get_total_mass() + self.mass = float( + total_mass_fun( + self.parametric_link_length_multipliers, + self.parametric_link_densities, + ).full() + ) + else: + self.mass = kin_dyn_object.get_total_mass() + self.parametric_link_densities = 0.0 self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier self.dcc_gain = settings.dcc_gain diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index b3548abd..f30d5f15 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -166,7 +166,7 @@ output = planner.solve() visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() - visualizer_settings.robot_model = planner_settings.robot_urdf + visualizer_settings.robot_model = planner.get_adam_model() visualizer_settings.considered_joints = planner_settings.joints_name_list visualizer_settings.contact_points = planner_settings.contact_points visualizer_settings.terrain = planner_settings.terrain diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index b6cdd972..eeb04f76 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -3,8 +3,14 @@ import logging import adam.casadi +import adam.model +import adam.numpy +import adam.parametric.casadi import casadi as cs import numpy as np +from adam.parametric.model.parametric_factories.parametric_model import ( + URDFParametricModelFactory, +) import hippopt as hp import hippopt.robot_planning as hp_rp @@ -14,6 +20,7 @@ class Settings: robot_urdf: str = dataclasses.field(default=None) joints_name_list: list[str] = dataclasses.field(default=None) + parametric_link_names: list[str] = dataclasses.field(default=None) contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) @@ -151,6 +158,10 @@ class Variables(hp.OptimizationObject): cls=hp.Variable, factory=hp_rp.HumanoidState ) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + parametric_link_length_multipliers: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + parametric_link_densities: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) references: References = hp.default_composite_field( cls=hp.Parameter, factory=References @@ -171,8 +182,11 @@ class Variables(hp.OptimizationObject): def __post_init__( self, settings: Settings, - kin_dyn_object: adam.casadi.KinDynComputations, - ): + kin_dyn_object: ( + adam.casadi.KinDynComputations + | adam.parametric.casadi.KinDynComputationsParametric + ), + ) -> None: self.state = hp_rp.HumanoidState( contact_point_descriptors=settings.contact_points, number_of_joints=len(settings.joints_name_list), @@ -181,7 +195,26 @@ def __post_init__( contact_point_descriptors=settings.contact_points, number_of_joints=len(settings.joints_name_list), ) - self.mass = kin_dyn_object.get_total_mass() + self.parametric_link_length_multipliers = ( + np.ones(len(settings.parametric_link_names)) + if settings.parametric_link_names is not None + else 0.0 + ) + + if isinstance( + kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ): + self.parametric_link_densities = kin_dyn_object.get_original_densities() + total_mass_fun = kin_dyn_object.get_total_mass() + self.mass = float( + total_mass_fun( + self.parametric_link_length_multipliers, + self.parametric_link_densities, + ).full() + ) + else: + self.mass = kin_dyn_object.get_total_mass() + self.parametric_link_densities = 0.0 self.gravity = settings.gravity self.static_friction = settings.static_friction self.relaxed_complementarity_epsilon = settings.relaxed_complementarity_epsilon @@ -196,13 +229,26 @@ def __init__(self, settings: Settings): self.settings = copy.deepcopy(settings) - self.kin_dyn_object = adam.casadi.KinDynComputations( - urdfstring=self.settings.robot_urdf, - joints_name_list=self.settings.joints_name_list, - root_link=self.settings.root_link, - gravity=self.settings.gravity, - f_opts=self.settings.casadi_function_options, - ) + if self.settings.parametric_link_names is not None: + self.parametric_model = True + self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + links_name_list=self.settings.parametric_link_names, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + else: + self.parametric_model = False + self.kin_dyn_object = adam.casadi.KinDynComputations( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + structure = Variables( settings=self.settings, kin_dyn_object=self.kin_dyn_object ) @@ -306,6 +352,8 @@ def _get_function_inputs_dict(self): "second_point_name": "p_1", "desired_yaw_name": "yd", "desired_height_name": "hd", + "parametric_link_length_multipliers_name": "pi_l", + "parametric_link_densities_name": "pi_d", "options": self.settings.casadi_function_options, } return function_inputs @@ -331,11 +379,20 @@ def _add_kinematics_constraints( com_kinematics_fun = hp_rp.center_of_mass_position_from_kinematics( kindyn_object=self.kin_dyn_object, **function_inputs ) - com_kinematics = com_kinematics_fun( - pb=variables.state.kinematics.base.position, - qb=normalized_quaternion, - s=variables.state.kinematics.joints.positions, - )["com_position"] + if self.parametric_model: + com_kinematics = com_kinematics_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, + )["com_position"] + else: + com_kinematics = com_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + )["com_position"] problem.add_constraint( expression=cs.MX(variables.state.com == com_kinematics), name="com_kinematics_consistency", @@ -404,12 +461,22 @@ def _add_kinematics_regularization( target_frame=self.settings.desired_frame_quaternion_cost_frame_name, **function_inputs, ) - rotation_error_kinematics = rotation_error_kinematics_fun( - pb=variables.state.kinematics.base.position, - qb=normalized_quaternion, - s=variables.state.kinematics.joints.positions, - qd=variables.references.frame_quaternion_xyzw, - )["rotation_error"] + if self.parametric_model: + rotation_error_kinematics = rotation_error_kinematics_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, + qd=variables.references.frame_quaternion_xyzw, + )["rotation_error"] + else: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + qd=variables.references.frame_quaternion_xyzw, + )["rotation_error"] problem.add_cost( expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), name="frame_rotation_error", @@ -464,12 +531,22 @@ def _add_contact_kinematic_consistency( ) # Consistency between the contact position and the kinematics - point_kinematics = point_kinematics_functions[descriptor.foot_frame]( - pb=variables.state.kinematics.base.position, - qb=normalized_quaternion, - s=variables.state.kinematics.joints.positions, - p_parent=descriptor.position_in_foot_frame, - )["point_position"] + if self.parametric_model: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + 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=descriptor.position_in_foot_frame, + )["point_position"] + else: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] problem.add_constraint( expression=cs.MX(point.p == point_kinematics), name=point.p.name() + "_kinematics_consistency", @@ -572,3 +649,22 @@ def set_references(self, references: References) -> None: def solve(self) -> hp.Output[Variables]: return self.op.problem.solve() + + def get_adam_model(self) -> adam.model.Model: + if self.parametric_model: + guess = self.get_initial_guess() + original_length = guess.parametric_link_length_multipliers + original_density = guess.parametric_link_densities + factory = URDFParametricModelFactory( + path=self.settings.robot_urdf, + math=adam.numpy.numpy_like.SpatialMath(), + links_name_list=self.settings.parametric_link_names, + length_multiplier=original_length, + densities=original_density, + ) + model = adam.model.Model.build( + factory=factory, joints_name_list=self.settings.joints_name_list + ) + return model + + return self.kin_dyn_object.rbdalgos.model