diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml index d00f5683..6d1db1f1 100644 --- a/.github/workflows/ci_cd.yml +++ b/.github/workflows/ci_cd.yml @@ -37,7 +37,7 @@ jobs: - name: Dependencies shell: bash -l {0} run: | - mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib + mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib resolve-robotics-uri-py mamba list - name: Install diff --git a/README.md b/README.md index 3a917313..5cf54c9b 100644 --- a/README.md +++ b/README.md @@ -7,19 +7,36 @@ hippopt is an open-source framework for generating whole-body trajectories for l ## Features -- [ ] Direct transcription of optimal control problems with multiple-shooting methods -- [ ] Support for floating-base robots, including humanoids and quadrupeds -- [ ] Integration with CasADi library for efficient numerical optimization -- [ ] Generation of optimized trajectories that include both kinematic and dynamic quantities -- [ ] Extensive documentation and examples to help you get started +- [X] Direct transcription of optimal control problems with multiple-shooting methods +- [X] Support for floating-base robots, including humanoids ... + - [ ] ... and quadrupeds +- [X] Integration with CasADi library for efficient numerical optimization +- [X] Generation of optimized trajectories that include both kinematic and dynamic quantities +- [ ] Extensive documentation +- [X] examples to help you get started ## Installation -It is suggested to use [``conda``](https://docs.conda.io/en/latest/). +It is suggested to use [``mamba``](https://github.com/conda-forge/miniforge). ```bash -conda install -c conda-forge casadi pytest +conda install -c conda-forge -c robotology python=3.11 casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib resolve-robotics-uri-py pip install --no-deps -e .[all] ``` +## Examples +### Turnkey planners +The folder [``turnkey_planners``](src/hippopt/turnkey_planners) contains examples of whole-body trajectory optimization for legged robots. +In this folder it is possible to find the following examples: +- [``humanoid_pose_finder/main.py``](src/hippopt/turnkey_planners/humanoid_pose_finder/main.py): generates a static pose for the humanoid robot ``ergoCub`` given desired foot and center of mass positions. +- [``humanoid_kinodynamic/main_single_step_flat_ground.py``](src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py): generates a kinodynamic trajectory for the humanoid robot ``ergoCub`` to perform a single step motion with no a-priori guess or terminal constraint. +- [``humanoid_kinodynamic/main_periodic_step.py``](src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py): generates a kinodynamic trajectory for the humanoid robot ``ergoCub`` to perform a periodic walking motion. +- [``humanoid_kinodynamic/main_walking_on_stairs.py``](src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py): generates a kinodynamic trajectory for the humanoid robot ``ergoCub`` to perform a walking motion on stairs. + +> [!IMPORTANT] +> For the tests to run, it is necessary to clone [``ergocub-software``](https://github.com/icub-tech-iit/ergocub-software) and extend the ``GAZEBO_MODEL_PATH`` environment variable to include the ``ergocub-software/urdf/ergoCub/robots`` and ``ergocub-software/urdf`` folders. + +> [!NOTE] +> It is necessary to launch the examples from a folder with write permissions, as the examples will generate several files (ground meshes, output videos, ...). + ## Citing this work If you find the work useful, please consider citing: diff --git a/setup.cfg b/setup.cfg index 9f5c1adf..094d3d31 100644 --- a/setup.cfg +++ b/setup.cfg @@ -64,6 +64,7 @@ robot_planning= adam-robotics turnkey_planners= idyntree + resolve-robotics-uri-py visualization= ffmpeg-python idyntree diff --git a/src/hippopt/turnkey_planners/__init__.py b/src/hippopt/turnkey_planners/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/.gitignore b/src/hippopt/turnkey_planners/humanoid_kinodynamic/.gitignore new file mode 100644 index 00000000..71007872 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/.gitignore @@ -0,0 +1,5 @@ +*.stl +*.urdf +frames/* +*.png +*.mp4 diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/__init__.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py new file mode 100644 index 00000000..19f14c0b --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -0,0 +1,498 @@ +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: + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/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.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, +) -> 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_input.set_references(pf_ref) + + 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, +) -> 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, + ) + + 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, +) -> 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, + ) + + +def compute_final_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> 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, + ) + + +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 = 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, + ) + + final_state = compute_final_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + 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, + ) + + 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 + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + print("Press [Enter] to visualize the initial guess.") + input() + + visualizer.visualize( + states=guess, + timestep_s=planner_settings.time_step, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_periodic_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 + 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] + 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", + ) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py new file mode 100644 index 00000000..31a47a83 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -0,0 +1,453 @@ +import logging + +import casadi as cs +import idyntree.bindings as idyntree +import liecasadi +import numpy as np +import resolve_robotics_uri_py + +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: + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/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 = 100.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 = 90.0 + settings.base_quaternion_cost_multiplier = 50.0 + settings.base_quaternion_velocity_cost_multiplier = 0.001 + settings.joint_regularization_cost_multiplier = 0.1 + 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.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": 1e0, + "acceptable_iter": 2, + "acceptable_compl_inf_tol": 1.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.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_initial_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, +) -> walking_variables.ExtendedHumanoidState: + 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 = np.array([0.0, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + 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_input.set_references(pf_ref) + + output_pf = pf_input.solve() + + output_state = walking_variables.ExtendedHumanoidState() + output_state.contact_points = output_pf.values.state.contact_points + output_state.kinematics = output_pf.values.state.kinematics + output_state.com = output_pf.values.state.com + + output_state.centroidal_momentum = np.zeros((6, 1)) + + return output_state + + +def compute_final_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, +) -> 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 = np.array([0.15, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + 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_input.set_references(pf_ref) + + output_pf = pf_input.solve() + return output_pf.values.state + + +def get_references( + input_settings: walking_settings.Settings, + desired_state: hp_rp.HumanoidState, +) -> walking_variables.References: + 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_state.kinematics.joints.positions + output_reference.com_linear_velocity = [0.1, 0.0, 0.0] + + return output_reference + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + + planner_settings = get_planner_settings() + planner = walking_planner.Planner(settings=planner_settings) + + pf_settings = get_pose_finder_settings(input_settings=planner_settings) + pf = pose_finder.Planner(settings=pf_settings) + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + + initial_state = compute_initial_state( + input_settings=planner_settings, + pf_input=pf, + ) + visualizer.visualize(initial_state) + + planner.set_initial_state(initial_state) + + final_state = compute_final_state( + input_settings=planner_settings, + pf_input=pf, + ) + + print("Press [Enter] to visualize the final desired state.") + input() + + visualizer.visualize(final_state) + + print("Starting the planner...") + + references = get_references( + input_settings=planner_settings, + desired_state=final_state, + ) + + planner.set_references(references) + + 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] + print("Press [Enter] to visualize the solution.") + input() + + plotter_settings = hp_rp.FootContactStatePlotterSettings() + plotter_settings.terrain = planner_settings.terrain + left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + + left_foot_plotter.plot_complementarity( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Complementarity", + blocking=False, + ) + right_foot_plotter.plot_complementarity( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Complementarity", + blocking=False, + ) + + left_foot_plotter.plot_forces( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Forces", + blocking=False, + ) + right_foot_plotter.plot_forces( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Forces", + blocking=False, + ) + + visualizer.visualize( + states=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=2.0, + ) + + visualizer.visualize( + states=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_single_step", + ) + + print("Press [Enter] to exit.") + input() + left_foot_plotter.close() + right_foot_plotter.close() diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py new file mode 100644 index 00000000..bfeb4762 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -0,0 +1,608 @@ +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_terrain(length: float, width: float, height: float) -> hp_rp.TerrainDescriptor: + first_step_origin = np.array([1.5 * length, 0.0, 0.0]) + second_step_origin = np.array([2 * length, 0.0, 0.0]) + + terrain = hp_rp.SmoothTerrain.step( + length=2 * length, width=width, height=height, position=first_step_origin + ) + hp_rp.SmoothTerrain.step( + length=0.9 * length, width=width, height=height, position=second_step_origin + ) + terrain.set_name(f"two_steps_{length}_{width}_{height}") + return terrain + + +def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.Settings: + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" + ) + settings = walking_settings.Settings() + settings.terrain = terrain + 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 = 50 + 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 = 150.0 + settings.dcc_epsilon = 0.01 + 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.5 + 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 = 0.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 = 1.0 + settings.force_regularization_cost_multiplier = 100.0 + settings.foot_yaw_regularization_cost_multiplier = 2000.0 + settings.swing_foot_height_cost_multiplier = 10.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.use_opti_callback = True + settings.opti_callback_save_costs = False + settings.opti_callback_save_constraint_multipliers = False + settings.casadi_function_options = {"cse": True} + settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True} + settings.casadi_solver_options = { + "max_iter": 10000, + "linear_solver": "mumps", + "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, + "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, + } + + 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.terrain = input_settings.terrain + settings.robot_urdf = input_settings.robot_urdf + settings.joints_name_list = input_settings.joints_name_list + + 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 = 1.0 + settings.static_friction = 0.3 + + 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.overwrite_terrain_files = True + 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, +) -> 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_input.set_references(pf_ref) + initial_guess = pf_input.get_initial_guess() + initial_guess.state = pf_ref.state + initial_guess.state.kinematics.base.position = initial_guess.state.com + pf_input.set_initial_guess(initial_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, +) -> 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, + ) + + 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_second_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[0].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, + ) + + +def compute_third_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> 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, + ) + + +def compute_final_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[1].transform + desired_right_foot_pose = contact_guess.right[2].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, + ) + + +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) + + step_length = 0.9 + step_height = 0.1 + swing_height = 0.1 + + planner_settings = get_planner_settings( + get_terrain(length=step_length / 2, width=0.8, height=step_height) + ) + 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 + + 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, swing_height + 2 * step_height]), + liecasadi.SO3.Identity(), + ), + force=np.array([0, 0, 100.0]), + activation_time=None, + deactivation_time=horizon * 3.0 / 7.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length, 0.1, 2 * step_height]), + liecasadi.SO3.Identity(), + ), + mid_swing_transform=None, + force=np.array([0, 0, 100.0]), + activation_time=horizon * 4.0 / 7.0, + deactivation_time=None, + ), + ] + + contact_phases_guess.right = [ + 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([0.25 * step_length, -0.1, swing_height + step_height]), + liecasadi.SO3.Identity(), + ), + force=np.array([0, 0, 100.0]), + activation_time=None, + deactivation_time=horizon / 7.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.5 * step_length, -0.1, step_height]), + liecasadi.SO3.Identity(), + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.75 * step_length, -0.1, swing_height + 2 * step_height]), + liecasadi.SO3.Identity(), + ), + force=np.array([0, 0, 100.0]), + activation_time=horizon * 2.0 / 7.0, + deactivation_time=horizon * 5.0 / 7.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length, -0.1, 2 * step_height]), + liecasadi.SO3.Identity(), + ), + mid_swing_transform=None, + force=np.array([0, 0, 100.0]), + activation_time=horizon * 6.0 / 7.0, + deactivation_time=None, + ), + ] + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + + initial_state = compute_initial_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + + final_state = compute_final_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + final_state.centroidal_momentum = np.zeros((6, 1)) + + second_state = compute_second_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + + third_state = compute_third_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + + # Until the middle of the second double support phase + first_part_guess_length = (planner_settings.horizon_length * 5) // 14 + first_part_guess = hp_rp.humanoid_state_interpolator( + initial_state=initial_state, + final_state=second_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=first_part_guess_length, + dt=planner_settings.time_step, + ) + + # Until the middle of the third double support phase + second_part_guess_length = (planner_settings.horizon_length * 2) // 7 + second_part_guess = hp_rp.humanoid_state_interpolator( + initial_state=second_state, + final_state=third_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=second_part_guess_length, + dt=planner_settings.time_step, + t0=first_part_guess_length * planner_settings.time_step, + ) + + # Remaining part + third_part_guess_length = ( + planner_settings.horizon_length + - first_part_guess_length + - second_part_guess_length + ) + third_part_guess = hp_rp.humanoid_state_interpolator( + initial_state=third_state, + final_state=final_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=third_part_guess_length, + dt=planner_settings.time_step, + t0=(first_part_guess_length + second_part_guess_length) + * planner_settings.time_step, + ) + + guess = first_part_guess + second_part_guess + third_part_guess + + print("Press [Enter] to visualize the initial guess.") + input() + + visualizer.visualize( + states=guess, + timestep_s=planner_settings.time_step, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_step_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 + 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] + 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_step", + ) + + plotter_settings = hp_rp.FootContactStatePlotterSettings() + plotter_settings.terrain = planner_settings.terrain + left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + + left_foot_plotter.plot_complementarity( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Complementarity", + blocking=False, + ) + right_foot_plotter.plot_complementarity( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Complementarity", + blocking=False, + ) + + print("Press [Enter] to exit.") + input() + left_foot_plotter.close() + right_foot_plotter.close() diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py new file mode 100644 index 00000000..d72db04e --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -0,0 +1,972 @@ +import copy + +import adam.casadi +import casadi as cs +import numpy as np + +import hippopt as hp +import hippopt.robot_planning as hp_rp +from hippopt.turnkey_planners.humanoid_kinodynamic.settings import Settings +from hippopt.turnkey_planners.humanoid_kinodynamic.variables import ( + ExtendedContactPoint, + ExtendedHumanoidState, + FootReferences, + References, + Variables, +) + + +class Planner: + 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() + + self.variables = Variables( + settings=self.settings, kin_dyn_object=self.kin_dyn_object + ) + + opti_callback = None + if self.settings.use_opti_callback: + opti_callback = ( + hp.opti_callback.BestCost() + & hp.opti_callback.AcceptablePrimalInfeasibility( + self.settings.acceptable_constraint_violation + ) + ) + + optimization_solver = hp.OptiSolver( + inner_solver=self.settings.opti_solver, + problem_type=self.settings.problem_type, + options_solver=self.settings.casadi_solver_options, + options_plugin=self.settings.casadi_opti_options, + callback_criterion=opti_callback, + ) + ocp_solver = hp.MultipleShootingSolver(optimization_solver=optimization_solver) + + self.ocp = hp.OptimalControlProblem.create( + input_structure=self.variables, + optimal_control_solver=ocp_solver, + horizon=self.settings.horizon_length, + ) + + sym = self.ocp.symbolic_structure # type: Variables + + function_inputs = self._get_function_inputs_dict() + + # Normalized quaternion computation + normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( + **function_inputs + ) + normalized_quaternion = normalized_quaternion_fun( + q=sym.system.kinematics.base.quaternion_xyzw + )["quaternion_normalized"] + + # Align names used in the terrain function with those in function_inputs + self.settings.terrain.change_options(**function_inputs) + + # Definition of contact constraint functions + dcc_planar_fun = hp_rp.dcc_planar_complementarity( + terrain=self.settings.terrain, + **function_inputs, + ) + dcc_margin_fun = hp_rp.dcc_complementarity_margin( + terrain=self.settings.terrain, + **function_inputs, + ) + friction_margin_fun = hp_rp.friction_cone_square_margin( + terrain=self.settings.terrain, **function_inputs + ) + height_fun = self.settings.terrain.height_function() + normal_force_fun = hp_rp.normal_force_component( + terrain=self.settings.terrain, **function_inputs + ) + + point_kinematics_functions = {} + all_contact_points = ( + sym.system.contact_points.left + sym.system.contact_points.right + ) + all_contact_initial_state = ( + sym.initial_state.contact_points.left + + sym.initial_state.contact_points.right + ) + + for i, point in enumerate(all_contact_points): + self._add_point_dynamics(point, initial_state=all_contact_initial_state[i]) + + self._add_contact_point_feasibility( + dcc_margin_fun, + dcc_planar_fun, + friction_margin_fun, + height_fun, + normal_force_fun, + point, + ) + + self._add_contact_kinematic_consistency( + function_inputs, + normalized_quaternion, + point, + point_kinematics_functions, + ) + + self._add_contact_point_regularization( + point=point, + desired_swing_height=sym.references.feet.desired_swing_height, + function_inputs=function_inputs, + ) + + self._add_robot_dynamics(all_contact_points, function_inputs) + + self._add_kinematics_constraints( + function_inputs, height_fun, normalized_quaternion + ) + self._add_kinematics_regularization( + function_inputs=function_inputs, + base_quaternion_normalized=normalized_quaternion, + ) + + self._add_contact_centroids_expressions(function_inputs) + + self._add_foot_regularization( + points=sym.system.contact_points.left, + descriptors=self.settings.contact_points.left, + references=sym.references.feet.left, + function_inputs=function_inputs, + foot_name="left", + ) + self._add_foot_regularization( + points=sym.system.contact_points.right, + descriptors=self.settings.contact_points.right, + references=sym.references.feet.right, + function_inputs=function_inputs, + foot_name="right", + ) + + self._add_periodicity_expression(all_contact_points) + + def _get_function_inputs_dict(self): + sym = self.ocp.symbolic_structure + function_inputs = { + "mass_name": sym.mass.name(), + "momentum_name": sym.system.centroidal_momentum.name(), + "com_name": sym.system.com.name(), + "quaternion_xyzw_name": "q", + "gravity_name": sym.gravity.name(), + "point_position_names": [], + "point_force_names": [], + "point_position_in_frame_name": "p_parent", + "base_position_name": "pb", + "base_quaternion_xyzw_name": "qb", + "joint_positions_name": "s", + "base_position_derivative_name": "pb_dot", + "base_quaternion_xyzw_derivative_name": "qb_dot", + "joint_velocities_name": "s_dot", + "point_position_name": "p", + "point_force_name": "f", + "point_velocity_name": "v", + "point_force_derivative_name": "f_dot", + "point_position_control_name": "u_p", + "height_multiplier_name": "kt", + "dcc_gain_name": "k_bs", + "dcc_epsilon_name": "eps", + "static_friction_name": "mu_s", + "desired_quaternion_xyzw_name": "qd", + "first_point_name": "p_0", + "second_point_name": "p_1", + "desired_yaw_name": "yd", + "desired_height_name": "hd", + "options": self.settings.casadi_function_options, + } + return function_inputs + + def _add_contact_centroids_expressions(self, function_inputs): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + + # Maximum feet relative height + def get_centroid( + points: list[ExtendedContactPoint], function_inputs_dict: dict + ) -> cs.MX: + function_inputs_dict["point_position_names"] = [ + pt.p.name() for pt in points + ] + point_position_dict = {pt.p.name(): pt.p for pt in points} + centroid_fun = hp_rp.contact_points_centroid( + number_of_points=len(function_inputs_dict["point_position_names"]), + **function_inputs_dict, + ) + return centroid_fun(**point_position_dict)["centroid"] + + left_centroid = get_centroid( + points=sym.system.contact_points.left, function_inputs_dict=function_inputs + ) + right_centroid = get_centroid( + points=sym.system.contact_points.right, function_inputs_dict=function_inputs + ) + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_feet_relative_height, + (left_centroid[2] - right_centroid[2]), + sym.maximum_feet_relative_height, + ), + apply_to_first_elements=False, + name="maximum_feet_relative_height", + ) + + # Contact centroid position cost + centroid_error = sym.references.contacts_centroid - 0.5 * ( + left_centroid + right_centroid + ) + weighted_centroid_squared_error = ( + centroid_error.T + @ cs.diag(sym.references.contacts_centroid_cost_weights) + @ centroid_error + ) + problem.add_expression_to_horizon( + expression=weighted_centroid_squared_error, + apply_to_first_elements=False, + name="contacts_centroid_cost", + mode=hp.ExpressionType.minimize, + scaling=self.settings.contacts_centroid_cost_multiplier, + ) + + def _add_kinematics_constraints( + self, + function_inputs: dict, + height_fun: cs.Function, + normalized_quaternion: cs.MX, + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + + # Unitary quaternion + problem.add_expression_to_horizon( + expression=cs.MX( + cs.sumsqr(sym.system.kinematics.base.quaternion_xyzw) == 1 + ), + apply_to_first_elements=False, + name="unitary_quaternion", + ) + + # Consistency of com position with kinematics + 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"] + problem.add_expression_to_horizon( + expression=cs.MX(sym.system.com == com_kinematics), + apply_to_first_elements=False, + name="com_kinematics_consistency", + ) + + # Consistency of centroidal momentum (angular part only) with kinematics + 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"] + problem.add_expression_to_horizon( + expression=cs.MX( + sym.system.centroidal_momentum[3:] + == centroidal_kinematics[3:] / sym.mass + ), + apply_to_first_elements=True, + name="centroidal_momentum_kinematics_consistency", + ) + + # Bounds on angular momentum + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_angular_momentum, + sym.system.centroidal_momentum[3:] * sym.mass, + sym.maximum_angular_momentum, + ), + apply_to_first_elements=True, + name="angular_momentum_bounds", + ) + + # Minimum com height + com_height = height_fun(p=sym.system.com)["point_height"] + problem.add_expression_to_horizon( + expression=cs.MX(com_height >= sym.minimum_com_height), + apply_to_first_elements=False, + name="minimum_com_height", + ) + + # Minimum feet lateral distance + left_frame = sym.system.contact_points.left[0].descriptor.foot_frame + right_frame = sym.system.contact_points.right[0].descriptor.foot_frame + relative_position_fun = hp_rp.frames_relative_position( + kindyn_object=self.kin_dyn_object, + reference_frame=right_frame, + target_frame=left_frame, + **function_inputs, + ) + 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, + name="minimum_feet_distance", + ) + + # Joint position bounds + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + sym.minimum_joint_positions, + sym.system.kinematics.joints.positions, + sym.maximum_joint_positions, + ), + apply_to_first_elements=False, + name="joint_position_bounds", + ) + + # Joint velocity bounds + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + sym.minimum_joint_velocities, + sym.system.kinematics.joints.velocities, + sym.maximum_joint_velocities, + ), + apply_to_first_elements=True, + name="joint_velocity_bounds", + ) + + # Final state + state_vectorized = sym.system.to_humanoid_state().to_list() + final_state_vectorized = ( + problem.final(state_el) for state_el in state_vectorized + ) + desired_final_state = sym.final_state.to_list() + desired_final_state_vectorized = ( + problem.initial(state_el) for state_el in desired_final_state + ) + + problem.add_expression( + mode=self.settings.final_state_expression_type, + expression=cs.MX( + cs.vertcat(*final_state_vectorized) + == cs.vertcat(*desired_final_state_vectorized) + ), + name="final_state_expression", + scaling=self.settings.final_state_expression_weight, + ) + + def _add_kinematics_regularization( + self, function_inputs: dict, base_quaternion_normalized: cs.MX + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + # Desired com velocity + com_velocity_error = ( + sym.system.centroidal_momentum[:3] - sym.references.com_linear_velocity + ) + com_velocity_weighted_error = ( + com_velocity_error.T + @ cs.diag(cs.DM(self.settings.com_linear_velocity_cost_weights)) + @ com_velocity_error + ) + problem.add_expression_to_horizon( + expression=com_velocity_weighted_error, + apply_to_first_elements=True, + name="com_velocity_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.com_linear_velocity_cost_multiplier, + ) + + # Desired frame orientation + rotation_error_kinematics_fun = hp_rp.rotation_error_from_kinematics( + kindyn_object=self.kin_dyn_object, + 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"] + problem.add_expression_to_horizon( + expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), + apply_to_first_elements=False, + name="frame_quaternion_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.desired_frame_quaternion_cost_multiplier, + ) + + # Desired base orientation + quaternion_error_fun = hp_rp.quaternion_xyzw_error(**function_inputs) + quaternion_error = quaternion_error_fun( + q=sym.system.kinematics.base.quaternion_xyzw, + qd=sym.references.base_quaternion_xyzw, + )["quaternion_error"] + problem.add_expression_to_horizon( + expression=cs.sumsqr(quaternion_error), + apply_to_first_elements=False, + name="base_quaternion_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.base_quaternion_cost_multiplier, + ) + + # Desired base angular velocity + problem.add_expression_to_horizon( + expression=cs.sumsqr( + sym.system.kinematics.base.quaternion_velocity_xyzw + - sym.references.base_quaternion_xyzw_velocity + ), + apply_to_first_elements=True, + name="base_quaternion_velocity_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.base_quaternion_velocity_cost_multiplier, + ) + + # Desired joint positions + joint_positions_error = ( + sym.system.kinematics.joints.positions - sym.references.joint_regularization + ) + joint_velocity_error = ( + sym.system.kinematics.joints.velocities + + cs.diag(cs.DM(self.settings.joint_regularization_cost_weights)) + * joint_positions_error + ) + problem.add_expression_to_horizon( + expression=cs.sumsqr(joint_velocity_error), + apply_to_first_elements=False, + name="joint_positions_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.joint_regularization_cost_multiplier, + ) + + def _add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + default_integrator = self.settings.integrator + + # dot(pb) = pb_dot (base position dynamics) + problem.add_dynamics( + hp.dot(sym.system.kinematics.base.position) + == sym.system.kinematics.base.linear_velocity, + x0=problem.initial(sym.initial_state.kinematics.base.position), + dt=sym.dt, + integrator=default_integrator, + name="base_position_dynamics", + ) + + # dot(q) = q_dot (base quaternion dynamics) + problem.add_dynamics( + hp.dot(sym.system.kinematics.base.quaternion_xyzw) + == sym.system.kinematics.base.quaternion_velocity_xyzw, + x0=problem.initial(sym.initial_state.kinematics.base.quaternion_xyzw), + dt=sym.dt, + integrator=default_integrator, + name="base_quaternion_dynamics", + ) + + # dot(s) = s_dot (joint position dynamics) + problem.add_dynamics( + hp.dot(sym.system.kinematics.joints.positions) + == sym.system.kinematics.joints.velocities, + x0=problem.initial(sym.initial_state.kinematics.joints.positions), + dt=sym.dt, + integrator=default_integrator, + name="joint_position_dynamics", + ) + + # dot(com) = h_g[:3] (center of mass dynamics, regularized by the mass) + problem.add_dynamics( + hp.dot(sym.system.com) == sym.system.centroidal_momentum[:3], # noqa + x0=problem.initial(sym.initial_state.com), # noqa + dt=sym.dt, + integrator=default_integrator, + name="com_dynamics", + ) + + # dot(h) = sum_i (p_i x f_i) + g (centroidal momentum dynamics,mass regularized) + function_inputs["point_position_names"] = [ + point.p.name() for point in all_contact_points + ] + function_inputs["point_force_names"] = [ + point.f.name() for point in all_contact_points + ] + centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( + number_of_points=len(function_inputs["point_position_names"]), + assume_unitary_mass=True, + **function_inputs, + ) + problem.add_dynamics( + hp.dot(sym.system.centroidal_momentum) == centroidal_dynamics, # noqa + x0=( + problem.initial(sym.initial_state.centroidal_momentum) + if self.settings.periodicity_expression_type is hp.ExpressionType.skip + else None + ), # noqa + dt=sym.dt, + integrator=default_integrator, + name="centroidal_momentum_dynamics", + ) + + def _add_contact_kinematic_consistency( + self, + function_inputs: dict, + normalized_quaternion: cs.MX, + point: ExtendedContactPoint, + point_kinematics_functions: dict, + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + + # Creation of contact kinematics consistency functions + descriptor = point.descriptor + if descriptor.foot_frame not in point_kinematics_functions: + point_kinematics_functions[descriptor.foot_frame] = ( + hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) + ) + + # 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"] + problem.add_expression_to_horizon( + expression=cs.MX(point.p == point_kinematics), + apply_to_first_elements=False, + name=point.p.name() + "_kinematics_consistency", + ) + + def _add_contact_point_feasibility( + self, + dcc_margin_fun: cs.Function, + dcc_planar_fun: cs.Function, + friction_margin_fun: cs.Function, + height_fun: cs.Function, + normal_force_fun: cs.Function, + point: ExtendedContactPoint, + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + + # Planar complementarity + dcc_planar = dcc_planar_fun( + p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v + )["planar_complementarity"] + problem.add_expression_to_horizon( + expression=cs.MX(point.v == dcc_planar), + apply_to_first_elements=True, + name=point.p.name() + "_planar_complementarity", + ) + + # Normal complementarity + dcc_margin = dcc_margin_fun( + p=point.p, + f=point.f, + v=point.v, + f_dot=point.f_dot, + k_bs=sym.dcc_gain, + eps=sym.dcc_epsilon, + )["dcc_complementarity_margin"] + problem.add_expression_to_horizon( + expression=cs.MX(dcc_margin >= 0), + apply_to_first_elements=True, + name=point.p.name() + "_dcc", + ) + + # Point height greater than zero + point_height = height_fun(p=point.p)["point_height"] + problem.add_expression_to_horizon( + expression=cs.MX(point_height >= 0), + apply_to_first_elements=False, + name=point.p.name() + "_height", + ) + + # Normal force greater than zero + normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] + problem.add_expression_to_horizon( + expression=cs.MX(normal_force >= 0), + apply_to_first_elements=False, + name=point.f.name() + "_normal", + ) + + # Friction + friction_margin = friction_margin_fun( + p=point.p, + f=point.f, + mu_s=sym.static_friction, + )["friction_cone_square_margin"] + problem.add_expression_to_horizon( + expression=cs.MX(friction_margin >= 0), + apply_to_first_elements=False, + name=point.f.name() + "_friction", + ) + + # Bounds on contact velocity control inputs + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_velocity_control, + point.u_v, + sym.maximum_velocity_control, + ), + apply_to_first_elements=True, + name=point.u_v.name() + "_bounds", # noqa + ) + + # Bounds on contact force control inputs + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_force_derivative, + point.f_dot * sym.mass, + sym.maximum_force_derivative, + ), + apply_to_first_elements=True, + name=point.f_dot.name() + "_bounds", # noqa + ) + + def _add_point_dynamics( + self, point: ExtendedContactPoint, initial_state: hp_rp.ContactPointState + ) -> None: + default_integrator = self.settings.integrator + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + + # dot(f) = f_dot + problem.add_dynamics( + hp.dot(point.f) == point.f_dot, + x0=problem.initial(initial_state.f), + dt=sym.dt, + integrator=default_integrator, + name=point.f.name() + "_dynamics", + ) + + # dot(p) = v + problem.add_dynamics( + hp.dot(point.p) == point.v, + x0=problem.initial(initial_state.p), + dt=sym.dt, + integrator=default_integrator, + name=point.p.name() + "_dynamics", + ) + + def _add_foot_regularization( + self, + points: list[ExtendedContactPoint], + descriptors: list[hp_rp.ContactPointDescriptor], + references: FootReferences, + function_inputs: dict, + foot_name: str, + ) -> None: + problem = self.ocp.problem + + # Force ratio regularization + sum_of_forces = cs.MX.zeros(3, 1) + for point in points: + sum_of_forces += point.f + + for i, point in enumerate(points): + alpha = references.points[i].desired_force_ratio + force_error = point.f - alpha * sum_of_forces + + problem.add_expression_to_horizon( + expression=cs.sumsqr(force_error), + apply_to_first_elements=False, + name=point.f.name() + "_regularization", + mode=hp.ExpressionType.minimize, + scaling=self.settings.force_regularization_cost_multiplier, + ) + + # Foot yaw task + centroid_position = np.zeros((3, 1)) + for descriptor in descriptors: + position_in_foot_frame = descriptor.position_in_foot_frame.reshape((3, 1)) + centroid_position += position_in_foot_frame + centroid_position /= len(descriptors) + + bottom_right_index = None + top_right_index = None + top_left_index = None + + # The values below are useful to get the outermost points of the foot + bottom_right_value = 0 + top_right_value = 0 + top_left_value = 0 + + for i, descriptor in enumerate(descriptors): + relative_position = ( + descriptor.position_in_foot_frame.reshape((3, 1)) - centroid_position + ) + + if ( + relative_position[1] < 0 + and relative_position[0] < 0 + and ( + bottom_right_index is None + or relative_position[0] * relative_position[1] > bottom_right_value + ) + ): + bottom_right_value = relative_position[0] * relative_position[1] + bottom_right_index = i + elif relative_position[1] < 0 < relative_position[0] and ( + top_right_index is None + or relative_position[0] * relative_position[1] > top_right_value + ): + top_right_value = relative_position[0] * relative_position[1] + top_right_index = i + elif ( + relative_position[1] > 0 + and relative_position[0] > 0 + and ( + top_left_index is None + or relative_position[0] * relative_position[1] > top_left_value + ) + ): + top_left_value = relative_position[0] * relative_position[1] + top_left_index = i + + assert bottom_right_index is not None + assert top_right_index is not None + assert top_left_index is not None + assert ( + bottom_right_index != top_right_index + and top_right_index != top_left_index + and top_left_index != bottom_right_index + ) + + yaw_alignment_fun = hp_rp.contact_points_yaw_alignment_error(**function_inputs) + yaw_alignment_forward = yaw_alignment_fun( + p_0=points[bottom_right_index].p, + p_1=points[top_right_index].p, + yd=references.yaw, + )["yaw_alignment_error"] + + yaw_alignment_sideways = yaw_alignment_fun( + p_0=points[top_right_index].p, + p_1=points[top_left_index].p, + yd=references.yaw + np.pi / 2, + )["yaw_alignment_error"] + + yaw_error = 0.5 * ( + cs.sumsqr(yaw_alignment_forward) + cs.sumsqr(yaw_alignment_sideways) + ) + + problem.add_expression_to_horizon( + expression=yaw_error, + apply_to_first_elements=False, + name=foot_name + "_yaw_regularization", + mode=hp.ExpressionType.minimize, + scaling=self.settings.foot_yaw_regularization_cost_multiplier, + ) + + def _add_contact_point_regularization( + self, + point: ExtendedContactPoint, + desired_swing_height: hp.StorageType, + function_inputs: dict, + ) -> None: + problem = self.ocp.problem + + # Swing height regularization + swing_heuristic_fun = hp_rp.swing_height_heuristic( + self.settings.terrain, **function_inputs + ) + heuristic = swing_heuristic_fun(p=point.p, v=point.v, hd=desired_swing_height)[ + "heuristic" + ] + + problem.add_expression_to_horizon( + expression=heuristic, + apply_to_first_elements=False, + name=point.p.name() + "_swing_height_regularization", + mode=hp.ExpressionType.minimize, + scaling=self.settings.swing_foot_height_cost_multiplier, + ) + + # Contact velocity control regularization + problem.add_expression_to_horizon( + expression=cs.sumsqr(point.u_v), + apply_to_first_elements=False, + name=point.u_v.name() + "_regularization", # noqa + mode=hp.ExpressionType.minimize, + scaling=self.settings.contact_velocity_control_cost_multiplier, + ) + + # Contact force control regularization + problem.add_expression_to_horizon( + expression=cs.sumsqr(point.f_dot), + apply_to_first_elements=False, + name=point.f_dot.name() + "_regularization", # noqa + mode=hp.ExpressionType.minimize, + scaling=self.settings.contact_force_control_cost_multiplier, + ) + + def _add_periodicity_expression(self, all_contact_points): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + initial_controls = [] + final_controls = [] + for point in all_contact_points: + initial_controls.append(problem.initial(point.u_v)) + initial_controls.append(problem.initial(point.f_dot)) + final_controls.append(problem.final(point.u_v)) + final_controls.append(problem.final(point.f_dot)) + initial_controls.append(problem.initial(sym.system.centroidal_momentum)) + final_controls.append(problem.final(sym.system.centroidal_momentum)) + initial_controls.append( + problem.initial(sym.system.kinematics.base.linear_velocity) + ) + initial_controls.append( + problem.initial(sym.system.kinematics.base.quaternion_velocity_xyzw) + ) + initial_controls.append( + problem.initial(sym.system.kinematics.joints.velocities) + ) + final_controls.append(problem.final(sym.system.kinematics.base.linear_velocity)) + final_controls.append( + problem.final(sym.system.kinematics.base.quaternion_velocity_xyzw) + ) + final_controls.append(problem.final(sym.system.kinematics.joints.velocities)) + problem.add_expression( + mode=self.settings.periodicity_expression_type, + expression=cs.MX( + cs.vertcat(*initial_controls) == cs.vertcat(*final_controls) + ), + name="periodicity_expression", + scaling=self.settings.periodicity_expression_weight, + ) + + def _apply_mass_regularization(self, input_var: Variables) -> Variables: + assert self.numeric_mass > 0 + output = input_var + if output.initial_state is not None: + if ( + output.initial_state.centroidal_momentum is not None + 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 + for point in ( + output.initial_state.contact_points.left + + output.initial_state.contact_points.right + ): + point.f /= self.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 + + if output.system is None: + return output + + system_list = ( + output.system if isinstance(output.system, list) else [output.system] + ) + + for system in system_list: + if system.centroidal_momentum is not None: + system.centroidal_momentum /= self.numeric_mass + for point in system.contact_points.left + system.contact_points.right: + point.f /= self.numeric_mass + + return output + + def _undo_mass_regularization(self, input_var: Variables) -> Variables: + assert self.numeric_mass > 0 + output = input_var + if output.initial_state is not None: + if ( + output.initial_state.centroidal_momentum is not None + 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 + for point in ( + output.initial_state.contact_points.left + + output.initial_state.contact_points.right + ): + point.f *= self.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 + + if output.system is None: + return output + + system_list = ( + output.system if isinstance(output.system, list) else [output.system] + ) + + for system in system_list: + if system.centroidal_momentum is not None: + system.centroidal_momentum *= self.numeric_mass + for point in system.contact_points.left + system.contact_points.right: + point.f *= self.numeric_mass + + return output + + def set_initial_guess(self, initial_guess: Variables) -> None: + self.ocp.problem.set_initial_guess( + self._apply_mass_regularization(initial_guess) + ) + + def get_initial_guess(self) -> Variables: + return self._undo_mass_regularization(self.ocp.problem.get_initial_guess()) + + def set_references(self, references: References | list[References]) -> None: + guess = ( + self.ocp.problem.get_initial_guess() + ) # Avoid the undo of the mass regularization + + assert isinstance(guess.references, list) + assert not isinstance(references, list) or len(references) == len( + guess.references + ) + for i in range(len(guess.references)): + guess.references[i] = ( + references[i] if isinstance(references, list) else references + ) + self.ocp.problem.set_initial_guess(guess) # Avoid the mass regularization + + def set_initial_state(self, initial_state: ExtendedHumanoidState) -> None: + guess = self.get_initial_guess() + guess.initial_state = initial_state + self.set_initial_guess(guess) + + def set_final_state(self, final_state: hp_rp.HumanoidState) -> None: + guess = self.get_initial_guess() + guess.final_state = final_state + self.set_initial_guess(guess) + + def solve(self) -> hp.Output[Variables]: + output = self.ocp.problem.solve() + output.values = self._undo_mass_regularization(output.values) + return output diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py new file mode 100644 index 00000000..51d15413 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -0,0 +1,143 @@ +import dataclasses +import typing + +import numpy as np + +import hippopt as hp +from hippopt import integrators as hp_int +from hippopt import robot_planning as hp_rp + + +@dataclasses.dataclass +class Settings: + robot_urdf: str = dataclasses.field(default=None) + joints_name_list: 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) + horizon_length: int = dataclasses.field(default=None) + time_step: float = dataclasses.field(default=None) + integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) + terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) + planar_dcc_height_multiplier: float = dataclasses.field(default=None) + dcc_gain: float = dataclasses.field(default=None) + dcc_epsilon: float = dataclasses.field(default=None) + static_friction: float = dataclasses.field(default=None) + maximum_velocity_control: np.ndarray = dataclasses.field(default=None) + maximum_force_derivative: np.ndarray = dataclasses.field(default=None) + maximum_angular_momentum: float = dataclasses.field(default=None) + minimum_com_height: float = dataclasses.field(default=None) + minimum_feet_lateral_distance: float = dataclasses.field(default=None) + maximum_feet_relative_height: float = dataclasses.field(default=None) + maximum_joint_positions: np.ndarray = dataclasses.field(default=None) + minimum_joint_positions: np.ndarray = dataclasses.field(default=None) + maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) + minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) + + final_state_expression_type: hp.ExpressionType = dataclasses.field(default=None) + final_state_expression_weight: float = dataclasses.field(default=None) + + periodicity_expression_type: hp.ExpressionType = dataclasses.field(default=None) + periodicity_expression_weight: float = dataclasses.field(default=None) + + contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) + + com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) + com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) + + desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) + + desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) + + joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) + joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + + force_regularization_cost_multiplier: float = dataclasses.field(default=None) + + foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) + + swing_foot_height_cost_multiplier: float = dataclasses.field(default=None) + + contact_velocity_control_cost_multiplier: float = dataclasses.field(default=None) + + contact_force_control_cost_multiplier: float = dataclasses.field(default=None) + + opti_solver: str = dataclasses.field(default="ipopt") + + problem_type: str = dataclasses.field(default="nlp") + + use_opti_callback: bool = dataclasses.field(default=None) + + acceptable_constraint_violation: float = dataclasses.field(default=None) + + opti_callback_save_costs: bool = dataclasses.field(default=None) + + opti_callback_save_constraint_multipliers: bool = dataclasses.field(default=None) + + casadi_function_options: dict = dataclasses.field(default_factory=dict) + + casadi_opti_options: dict = dataclasses.field(default_factory=dict) + + 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 + + def is_valid(self) -> bool: + 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 + ) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py new file mode 100644 index 00000000..a0978f61 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -0,0 +1,344 @@ +import dataclasses +from typing import TypeVar + +import adam.casadi +import numpy as np + +import hippopt as hp +from hippopt import robot_planning as hp_rp +from hippopt.turnkey_planners.humanoid_kinodynamic.settings import Settings + + +@dataclasses.dataclass +class ContactReferences(hp.OptimizationObject): + desired_force_ratio: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_points: int) -> None: + self.desired_force_ratio = ( + 1.0 / number_of_points + if number_of_points is not None and number_of_points > 0 + else 0.0 + ) + + +@dataclasses.dataclass +class FootReferences(hp.OptimizationObject): + points: hp.CompositeType[list[ContactReferences]] = hp.default_composite_field( + factory=list, time_varying=False + ) + yaw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=0) + + def __post_init__(self, number_of_points: int) -> None: + number_of_points = number_of_points if number_of_points is not None else 0 + self.points = [ + ContactReferences(number_of_points=number_of_points) + for _ in range(number_of_points) + ] + + self.yaw = 0.0 + + +@dataclasses.dataclass +class FeetReferences(hp.OptimizationObject): + left: hp.CompositeType[FootReferences] = hp.default_composite_field( + factory=FootReferences, time_varying=False + ) + right: hp.CompositeType[FootReferences] = hp.default_composite_field( + factory=FootReferences, time_varying=False + ) + + desired_swing_height: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) + + def __post_init__( + self, number_of_points_left: int, number_of_points_right: int + ) -> None: + self.left = FootReferences(number_of_points=number_of_points_left) + self.right = FootReferences(number_of_points=number_of_points_right) + self.desired_swing_height = 0.02 + + +@dataclasses.dataclass +class References(hp.OptimizationObject): + feet: hp.CompositeType[FeetReferences] = hp.default_composite_field( + factory=FeetReferences, time_varying=False + ) + + contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + contacts_centroid: hp.StorageType = hp.default_storage_field(hp.Parameter) + + com_linear_velocity: hp.StorageType = hp.default_storage_field(hp.Parameter) + + desired_frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + + base_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + base_quaternion_xyzw_velocity: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + + joint_regularization: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) + + def __post_init__( + self, + number_of_joints: int, + number_of_points_left: int, + number_of_points_right: int, + ) -> None: + self.feet = FeetReferences( + number_of_points_left=number_of_points_left, + number_of_points_right=number_of_points_right, + ) + self.contacts_centroid_cost_weights = np.zeros((3, 1)) + self.contacts_centroid = np.zeros((3, 1)) + self.com_linear_velocity = np.zeros((3, 1)) + self.desired_frame_quaternion_xyzw = np.zeros((4, 1)) + self.desired_frame_quaternion_xyzw[3] = 1 + self.base_quaternion_xyzw = np.zeros((4, 1)) + self.base_quaternion_xyzw[3] = 1 + self.base_quaternion_xyzw_velocity = np.zeros((4, 1)) + self.joint_regularization = np.zeros((number_of_joints, 1)) + + +TExtendedContactPoint = TypeVar("TExtendedContactPoint", bound="ExtendedContactPoint") + + +@dataclasses.dataclass +class ExtendedContactPoint( + hp_rp.ContactPointState, + hp_rp.ContactPointStateDerivative, +): + u_v: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: + hp_rp.ContactPointState.__post_init__(self, input_descriptor) + hp_rp.ContactPointStateDerivative.__post_init__(self) + self.u_v = np.zeros(3) + + def to_contact_point_state(self) -> hp_rp.ContactPointState: + output = hp_rp.ContactPointState() + output.p = self.p + output.f = self.f + output.descriptor = self.descriptor + return output + + @staticmethod + def from_contact_point_state( + input_state: hp_rp.ContactPointState, + ) -> TExtendedContactPoint: + output = ExtendedContactPoint(input_descriptor=input_state.descriptor) + output.p = input_state.p + output.f = input_state.f + output.u_v = None + output.f_dot = None + output.v = None + return output + + +@dataclasses.dataclass +class FeetContactPointsExtended(hp.OptimizationObject): + left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + + def to_feet_contact_points(self) -> hp_rp.FeetContactPoints: + output = hp_rp.FeetContactPoints() + output.left = hp_rp.FootContactState.from_list( + [point.to_contact_point_state() for point in self.left] + ) + output.right = hp_rp.FootContactState.from_list( + [point.to_contact_point_state() for point in self.right] + ) + return output + + def from_feet_contact_points(self, input_points: hp_rp.FeetContactPoints) -> None: + self.left = [ + ExtendedContactPoint.from_contact_point_state(point) + for point in input_points.left + ] + self.right = [ + ExtendedContactPoint.from_contact_point_state(point) + for point in input_points.right + ] + + +TExtendedHumanoid = TypeVar("TExtendedHumanoid", bound="ExtendedHumanoid") + + +@dataclasses.dataclass +class ExtendedHumanoid(hp.OptimizationObject): + contact_points: hp.CompositeType[FeetContactPointsExtended] = ( + hp.default_composite_field(factory=FeetContactPointsExtended) + ) + + kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( + cls=hp.Variable, factory=hp_rp.FloatingBaseSystem + ) + + com: hp.StorageType = hp.default_storage_field(hp.Variable) + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + + contact_point_descriptors: dataclasses.InitVar[ + hp_rp.FeetContactPointDescriptors + ] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + if contact_point_descriptors is not None: + self.contact_points.left = [ + ExtendedContactPoint(input_descriptor=point) + for point in contact_point_descriptors.left + ] + self.contact_points.right = [ + ExtendedContactPoint(input_descriptor=point) + for point in contact_point_descriptors.right + ] + + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(6) + self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) + + def to_humanoid_state(self) -> hp_rp.HumanoidState: + output = hp_rp.HumanoidState() + output.kinematics = self.kinematics.to_floating_base_system_state() + output.contact_points = self.contact_points.to_feet_contact_points() + output.com = self.com + return output + + @staticmethod + def from_humanoid_state(input_state: hp_rp.HumanoidState) -> TExtendedHumanoid: + output = ExtendedHumanoid() + output.contact_points.from_feet_contact_points(input_state.contact_points) + output.kinematics = hp_rp.FloatingBaseSystem.from_floating_base_system_state( + input_state.kinematics + ) + output.com = input_state.com + output.centroidal_momentum = None + return output + + +@dataclasses.dataclass +class ExtendedHumanoidState(hp_rp.HumanoidState): + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + hp_rp.HumanoidState.__post_init__( + self, + contact_point_descriptors=contact_point_descriptors, + number_of_joints=number_of_joints, + ) + self.centroidal_momentum = np.zeros(6) + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + system: hp.CompositeType[ExtendedHumanoid] = hp.default_composite_field( + cls=hp.Variable, factory=ExtendedHumanoid + ) + + mass: 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 + ) + + final_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False + ) + + dt: hp.StorageType = hp.default_storage_field(hp.Parameter) + gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) + planar_dcc_height_multiplier: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + dcc_gain: hp.StorageType = hp.default_storage_field(hp.Parameter) + dcc_epsilon: hp.StorageType = hp.default_storage_field(hp.Parameter) + static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_velocity_control: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_force_derivative: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_angular_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_com_height: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_feet_lateral_distance: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + maximum_feet_relative_height: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) + + references: hp.CompositeType[References] = hp.default_composite_field( + factory=References, time_varying=True + ) + + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) + kin_dyn_object: dataclasses.InitVar[adam.casadi.KinDynComputations] = ( + dataclasses.field(default=None) + ) + + def __post_init__( + self, + settings: Settings, + kin_dyn_object: adam.casadi.KinDynComputations, + ) -> None: + self.system = ExtendedHumanoid( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + + self.initial_state = ExtendedHumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + + self.final_state = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + + self.dt = settings.time_step + self.gravity = kin_dyn_object.g + self.mass = kin_dyn_object.get_total_mass() + + self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier + self.dcc_gain = settings.dcc_gain + self.dcc_epsilon = settings.dcc_epsilon + self.static_friction = settings.static_friction + self.maximum_velocity_control = settings.maximum_velocity_control + self.maximum_force_derivative = settings.maximum_force_derivative + self.maximum_angular_momentum = settings.maximum_angular_momentum + self.minimum_com_height = settings.minimum_com_height + self.minimum_feet_lateral_distance = settings.minimum_feet_lateral_distance + self.maximum_feet_relative_height = settings.maximum_feet_relative_height + self.maximum_joint_positions = settings.maximum_joint_positions + self.minimum_joint_positions = settings.minimum_joint_positions + self.maximum_joint_velocities = settings.maximum_joint_velocities + self.minimum_joint_velocities = settings.minimum_joint_velocities + self.references = References( + number_of_joints=kin_dyn_object.NDoF, + number_of_points_left=len(settings.contact_points.left), + number_of_points_right=len(settings.contact_points.right), + ) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore b/src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore new file mode 100644 index 00000000..f48bb6bf --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore @@ -0,0 +1,2 @@ +*.stl +*.urdf diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/__init__.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py new file mode 100644 index 00000000..b3548abd --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -0,0 +1,207 @@ +import logging + +import casadi as cs +import idyntree.bindings as idyntree +import liecasadi +import numpy as np +import resolve_robotics_uri_py + +import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder + +if __name__ == "__main__": + logging.basicConfig(level=logging.DEBUG) + + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" + ) + + planner_settings = pose_finder.Settings() + planner_settings.robot_urdf = str(urdf_path) + planner_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(planner_settings.joints_name_list) + + idyntree_model_loader = idyntree.ModelLoader() + idyntree_model_loader.loadReducedModelFromFile( + planner_settings.robot_urdf, planner_settings.joints_name_list + ) + idyntree_model = idyntree_model_loader.model() + + planner_settings.root_link = "root_link" + planner_settings.desired_frame_quaternion_cost_frame_name = "chest" + + planner_settings.contact_points = hp_rp.FeetContactPointDescriptors() + planner_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]), + ) + ) + planner_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]), + ) + ) + + planner_settings.relaxed_complementarity_epsilon = 0.0001 + planner_settings.static_friction = 0.3 + + planner_settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) + planner_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(): + planner_settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) + planner_settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) + + planner_settings.joint_regularization_cost_weights = np.ones(number_of_joints) + planner_settings.joint_regularization_cost_weights[:3] = 0.1 # torso + planner_settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + planner_settings.joint_regularization_cost_weights[11:] = 1.0 # legs + + planner_settings.base_quaternion_cost_multiplier = 50.0 + planner_settings.desired_frame_quaternion_cost_multiplier = 100.0 + planner_settings.joint_regularization_cost_multiplier = 0.1 + planner_settings.force_regularization_cost_multiplier = 0.2 + planner_settings.com_regularization_cost_multiplier = 10.0 + planner_settings.average_force_regularization_cost_multiplier = 10.0 + planner_settings.point_position_regularization_cost_multiplier = 100.0 + planner_settings.casadi_function_options = {} + planner_settings.casadi_opti_options = {} + planner_settings.casadi_solver_options = {} + + planner = pose_finder.Planner(settings=planner_settings) + + references = pose_finder.References( + contact_point_descriptors=planner_settings.contact_points, + number_of_joints=number_of_joints, + ) + + references.state.com = np.array([0.0, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + references.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + references.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + references.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + references.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + + references.state.kinematics.joints.positions = 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, + ] + ) + + planner.set_references(references) + + output = planner.solve() + + visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() + visualizer_settings.robot_model = planner_settings.robot_urdf + visualizer_settings.considered_joints = planner_settings.joints_name_list + visualizer_settings.contact_points = planner_settings.contact_points + visualizer_settings.terrain = planner_settings.terrain + visualizer_settings.working_folder = "./" + + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + visualizer.visualize(output.values.state) # noqa + + print("Press [Enter] to move to the next pose.") + input() + + references.com = np.array([0.15, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + references.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + references.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + planner.set_references(references) + + output = planner.solve() + visualizer.visualize(output.values.state) + + print("Press [Enter] to close.") + input() diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py new file mode 100644 index 00000000..b6cdd972 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -0,0 +1,574 @@ +import copy +import dataclasses +import logging + +import adam.casadi +import casadi as cs +import numpy as np + +import hippopt as hp +import hippopt.robot_planning as hp_rp + + +@dataclasses.dataclass +class Settings: + robot_urdf: str = dataclasses.field(default=None) + joints_name_list: 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) + terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) + relaxed_complementarity_epsilon: float = dataclasses.field(default=None) + static_friction: float = dataclasses.field(default=None) + maximum_joint_positions: np.ndarray = dataclasses.field(default=None) + minimum_joint_positions: np.ndarray = dataclasses.field(default=None) + + base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) + + desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + com_regularization_cost_multiplier: float = dataclasses.field(default=None) + + joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) + joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + + force_regularization_cost_multiplier: float = dataclasses.field(default=None) + average_force_regularization_cost_multiplier: float = dataclasses.field( + default=None + ) + + point_position_regularization_cost_multiplier: float = 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) + casadi_opti_options: dict = dataclasses.field(default_factory=dict) + 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.terrain = hp_rp.PlanarTerrain() + self.relaxed_complementarity_epsilon = 0.0001 + self.static_friction = 0.3 + + def is_valid(self) -> bool: + ok = True + logger = logging.getLogger("[hippopt::HumanoidPoseFInder::Settings]") + 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.root_link is None: + logger.error("root_link is None") + ok = False + if self.gravity is None or len(self.gravity) != 6: + logger.error("gravity is None") + ok = False + if self.terrain is None: + logger.error("terrain is None") + ok = False + if self.relaxed_complementarity_epsilon is None: + logger.error("relaxed_complementarity_epsilon is None") + ok = False + if self.static_friction is None: + logger.error("static_friction 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.com_regularization_cost_multiplier is None: + logger.error("com_regularization_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.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.joint_regularization_cost_weights is None: + logger.error("joint_regularization_cost_weights is None") + 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.average_force_regularization_cost_multiplier is None: + logger.error("average_force_regularization_cost_multiplier is None") + ok = False + if self.point_position_regularization_cost_multiplier is None: + logger.error("point_position_regularization_cost_multiplier is None") + ok = False + return ok + + +@dataclasses.dataclass +class References(hp.OptimizationObject): + state: hp_rp.HumanoidState = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState + ) + frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + contact_point_descriptors: dataclasses.InitVar[ + hp_rp.FeetContactPointDescriptors + ] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ): + self.state = hp_rp.HumanoidState( + contact_point_descriptors=contact_point_descriptors, + number_of_joints=number_of_joints, + ) + self.frame_quaternion_xyzw = np.zeros(4) + self.frame_quaternion_xyzw[3] = 1.0 + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + state: hp_rp.HumanoidState = hp.default_composite_field( + cls=hp.Variable, factory=hp_rp.HumanoidState + ) + mass: 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 + ) + + relaxed_complementarity_epsilon: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_positions: 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) + ) + + def __post_init__( + self, + settings: Settings, + kin_dyn_object: adam.casadi.KinDynComputations, + ): + self.state = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=len(settings.joints_name_list), + ) + self.references = References( + contact_point_descriptors=settings.contact_points, + number_of_joints=len(settings.joints_name_list), + ) + self.mass = kin_dyn_object.get_total_mass() + self.gravity = settings.gravity + self.static_friction = settings.static_friction + self.relaxed_complementarity_epsilon = settings.relaxed_complementarity_epsilon + self.maximum_joint_positions = settings.maximum_joint_positions + self.minimum_joint_positions = settings.minimum_joint_positions + + +class Planner: + def __init__(self, settings: Settings): + 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, + ) + structure = Variables( + settings=self.settings, kin_dyn_object=self.kin_dyn_object + ) + + optimization_solver = hp.OptiSolver( + inner_solver=self.settings.opti_solver, + problem_type=self.settings.problem_type, + options_solver=self.settings.casadi_solver_options, + options_plugin=self.settings.casadi_opti_options, + ) + + self.op = hp.OptimizationProblem.create( + input_structure=structure, optimization_solver=optimization_solver + ) + + variables = self.op.variables # type: Variables + + function_inputs = self._get_function_inputs_dict() + + # Normalized quaternion computation + normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( + **function_inputs + ) + normalized_quaternion = normalized_quaternion_fun( + q=variables.state.kinematics.base.quaternion_xyzw + )["quaternion_normalized"] + + # Align names used in the terrain function with those in function_inputs + self.settings.terrain.change_options(**function_inputs) + + # Definition of contact constraint functions + relaxed_complementarity_fun = hp_rp.relaxed_complementarity_margin( + terrain=self.settings.terrain, + **function_inputs, + ) + friction_margin_fun = hp_rp.friction_cone_square_margin( + terrain=self.settings.terrain, **function_inputs + ) + height_fun = self.settings.terrain.height_function() + normal_force_fun = hp_rp.normal_force_component( + terrain=self.settings.terrain, **function_inputs + ) + + point_kinematics_functions = {} + all_contact_points = ( + variables.state.contact_points.left + variables.state.contact_points.right + ) + + for i, point in enumerate(all_contact_points): + self._add_contact_point_feasibility( + relaxed_complementarity_fun, + friction_margin_fun, + height_fun, + normal_force_fun, + point, + ) + + self._add_contact_kinematic_consistency( + function_inputs, + normalized_quaternion, + point, + point_kinematics_functions, + ) + + self._add_kinematics_constraints( + function_inputs, normalized_quaternion, all_contact_points + ) + self._add_kinematics_regularization( + function_inputs=function_inputs, normalized_quaternion=normalized_quaternion + ) + + self._add_foot_regularization( + points=variables.state.contact_points.left, + references=variables.references.state.contact_points.left, + ) + self._add_foot_regularization( + points=variables.state.contact_points.right, + references=variables.references.state.contact_points.right, + ) + + def _get_function_inputs_dict(self): + variables = self.op.variables + function_inputs = { + "mass_name": variables.mass.name(), + "com_name": variables.state.com.name(), + "quaternion_xyzw_name": "q", + "gravity_name": variables.gravity.name(), + "point_position_names": [], + "point_force_names": [], + "point_position_in_frame_name": "p_parent", + "base_position_name": "pb", + "base_quaternion_xyzw_name": "qb", + "joint_positions_name": "s", + "point_position_name": "p", + "point_force_name": "f", + "height_multiplier_name": "kt", + "relaxed_complementarity_epsilon_name": "eps", + "static_friction_name": "mu_s", + "desired_quaternion_xyzw_name": "qd", + "first_point_name": "p_0", + "second_point_name": "p_1", + "desired_yaw_name": "yd", + "desired_height_name": "hd", + "options": self.settings.casadi_function_options, + } + return function_inputs + + def _add_kinematics_constraints( + self, + function_inputs: dict, + normalized_quaternion: cs.MX, + all_contact_points: list[hp_rp.ContactPointState], + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Unitary quaternion + problem.add_constraint( + expression=cs.MX( + cs.sumsqr(variables.state.kinematics.base.quaternion_xyzw) == 1 + ), + name="unitary_quaternion", + ) + + # Consistency of com position with kinematics + 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"] + problem.add_constraint( + expression=cs.MX(variables.state.com == com_kinematics), + name="com_kinematics_consistency", + ) + + # Zero momentum derivative + # 0 = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) + function_inputs["point_position_names"] = [ + point.p.name() for point in all_contact_points + ] + function_inputs["point_force_names"] = [ + point.f.name() for point in all_contact_points + ] + centroidal_dynamics_fun = hp_rp.centroidal_dynamics_with_point_forces( + number_of_points=len(function_inputs["point_position_names"]), + **function_inputs, + ) + + centroidal_inputs = { + variables.mass.name(): variables.mass, # noqa + variables.gravity.name(): variables.gravity, # noqa + variables.state.com.name(): variables.state.com, + } + for point in all_contact_points: + centroidal_inputs[point.p.name()] = point.p + centroidal_inputs[point.f.name()] = point.f + + centroidal_dynamics = centroidal_dynamics_fun(**centroidal_inputs)["h_g_dot"] + + problem.add_constraint( + expression=centroidal_dynamics == np.zeros(6), # noqa + name="centroidal_momentum_dynamics", + ) + + # Joint position bounds + problem.add_constraint( + expression=cs.Opti_bounded( + variables.minimum_joint_positions, + variables.state.kinematics.joints.positions, + variables.maximum_joint_positions, + ), + name="joint_position_bounds", + ) + + def _add_kinematics_regularization( + self, function_inputs: dict, normalized_quaternion: cs.MX + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Desired base orientation + quaternion_error_fun = hp_rp.quaternion_xyzw_error(**function_inputs) + quaternion_error = quaternion_error_fun( + q=variables.state.kinematics.base.quaternion_xyzw, + qd=variables.references.state.kinematics.base.quaternion_xyzw, + )["quaternion_error"] + problem.add_cost( + expression=cs.sumsqr(quaternion_error), + name="base_quaternion_error", + scaling=self.settings.base_quaternion_cost_multiplier, + ) + + # Desired frame orientation + rotation_error_kinematics_fun = hp_rp.rotation_error_from_kinematics( + kindyn_object=self.kin_dyn_object, + 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"] + problem.add_cost( + expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), + name="frame_rotation_error", + scaling=self.settings.desired_frame_quaternion_cost_multiplier, + ) + + # Desired center of mass position + com_position_error = variables.state.com - variables.references.state.com + problem.add_cost( + expression=cs.sumsqr(com_position_error), + name="com_position_error", + scaling=self.settings.com_regularization_cost_multiplier, + ) + + # Desired joint positions + joint_positions_error = ( + variables.state.kinematics.joints.positions + - variables.references.state.kinematics.joints.positions + ) + + weighted_error = ( + joint_positions_error.T + @ cs.diag(self.settings.joint_regularization_cost_weights) + @ joint_positions_error + ) + + problem.add_cost( + expression=weighted_error, + name="joint_positions_error", + scaling=self.settings.joint_regularization_cost_multiplier, + ) + + def _add_contact_kinematic_consistency( + self, + function_inputs: dict, + normalized_quaternion: cs.MX, + point: hp_rp.ContactPointState, + point_kinematics_functions: dict, + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Creation of contact kinematics consistency functions + descriptor = point.descriptor + if descriptor.foot_frame not in point_kinematics_functions: + point_kinematics_functions[descriptor.foot_frame] = ( + hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) + ) + + # 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"] + problem.add_constraint( + expression=cs.MX(point.p == point_kinematics), + name=point.p.name() + "_kinematics_consistency", + ) + + def _add_contact_point_feasibility( + self, + complementarity_margin_fun: cs.Function, + friction_margin_fun: cs.Function, + height_fun: cs.Function, + normal_force_fun: cs.Function, + point: hp_rp.ContactPointState, + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Normal complementarity + dcc_margin = complementarity_margin_fun( + p=point.p, + f=point.f, + eps=variables.relaxed_complementarity_epsilon, + )["relaxed_complementarity_margin"] + problem.add_constraint( + expression=cs.MX(dcc_margin >= 0), + name=point.p.name() + "_complementarity", + ) + + # Point height greater than zero + point_height = height_fun(p=point.p)["point_height"] + problem.add_constraint( + expression=cs.MX(point_height >= 0), + name=point.p.name() + "_height", + ) + + # Normal force greater than zero + normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] + problem.add_constraint( + expression=cs.MX(normal_force >= 0), + name=point.f.name() + "_normal", + ) + + # Friction + friction_margin = friction_margin_fun( + p=point.p, + f=point.f, + mu_s=variables.static_friction, + )["friction_cone_square_margin"] + problem.add_constraint( + expression=cs.MX(friction_margin >= 0), + name=point.f.name() + "_friction", + ) + + def _add_foot_regularization( + self, + points: list[hp_rp.ContactPointState], + references: list[hp_rp.ContactPointState], + ) -> None: + problem = self.op.problem + + assert len(points) > 0 + # Force ratio regularization + sum_of_forces = cs.MX.zeros(3, 1) + for point in points: + sum_of_forces += point.f + + multiplier = 1.0 / len(points) + + for point in points: + force_error_from_average = point.f - multiplier * sum_of_forces + + problem.add_cost( + expression=cs.sumsqr(force_error_from_average), + name=point.f.name() + "_average_regularization", + scaling=self.settings.average_force_regularization_cost_multiplier, + ) + + # Force and position regularization + for i, point in enumerate(points): + problem.add_cost( + expression=cs.sumsqr(point.p - references[i].p), + name=point.p.name() + "_regularization", + scaling=self.settings.point_position_regularization_cost_multiplier, + ) + problem.add_cost( + expression=cs.sumsqr(point.f - references[i].f), + name=point.f.name() + "_regularization", + scaling=self.settings.force_regularization_cost_multiplier, + ) + + def set_initial_guess(self, initial_guess: Variables) -> None: + self.op.problem.set_initial_guess(initial_guess) + + def get_initial_guess(self) -> Variables: + return self.op.problem.get_initial_guess() + + def set_references(self, references: References) -> None: + guess = self.get_initial_guess() + guess.references = references + self.set_initial_guess(guess) + + def solve(self) -> hp.Output[Variables]: + return self.op.problem.solve()