From 88af29d3aa6950408102d1b440026134f50dfc6b Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 26 Feb 2024 13:08:27 +0100 Subject: [PATCH 01/16] Changed logic in the settings of the humanoid kinodymanic planner to set default values only if they originally None --- .../humanoid_kinodynamic/settings.py | 75 ++++++++++++++----- 1 file changed, 56 insertions(+), 19 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index 51d15413..78d39d98 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -85,25 +85,62 @@ class Settings: casadi_solver_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: - self.root_link = "root_link" - self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) - self.integrator = hp_int.ImplicitTrapezoid - self.terrain = hp_rp.PlanarTerrain() - self.planar_dcc_height_multiplier = 10.0 - self.dcc_gain = 20.0 - self.dcc_epsilon = 0.05 - self.static_friction = 0.3 - self.maximum_velocity_control = np.array([2.0, 2.0, 5.0]) - self.maximum_force_derivative = np.array([100.0, 100.0, 100.0]) - self.maximum_angular_momentum = 10.0 - self.final_state_expression_type = hp.ExpressionType.skip - self.final_state_expression_weight = 1.0 - self.periodicity_expression_type = hp.ExpressionType.skip - self.periodicity_expression_weight = 1.0 - self.use_opti_callback = False - self.acceptable_constraint_violation = 1e-3 - self.opti_callback_save_costs = True - self.opti_callback_save_constraint_multipliers = True + if self.root_link is None: + self.root_link = "root_link" + + if self.gravity is None: + self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) + + if self.integrator is None: + self.integrator = hp_int.ImplicitTrapezoid + + if self.terrain is None: + self.terrain = hp_rp.PlanarTerrain() + + if self.planar_dcc_height_multiplier is None: + self.planar_dcc_height_multiplier = 10.0 + + if self.dcc_gain is None: + self.dcc_gain = 20.0 + + if self.dcc_epsilon is None: + self.dcc_epsilon = 0.05 + + if self.static_friction is None: + self.static_friction = 0.3 + + if self.maximum_velocity_control is None: + self.maximum_velocity_control = np.array([2.0, 2.0, 5.0]) + + if self.maximum_force_derivative is None: + self.maximum_force_derivative = np.array([100.0, 100.0, 100.0]) + + if self.maximum_angular_momentum is None: + self.maximum_angular_momentum = 10.0 + + if self.final_state_expression_type is None: + self.final_state_expression_type = hp.ExpressionType.skip + + if self.final_state_expression_weight is None: + self.final_state_expression_weight = 1.0 + + if self.periodicity_expression_type is None: + self.periodicity_expression_type = hp.ExpressionType.skip + + if self.periodicity_expression_weight is None: + self.periodicity_expression_weight = 1.0 + + if self.use_opti_callback is None: + self.use_opti_callback = False + + if self.acceptable_constraint_violation is None: + self.acceptable_constraint_violation = 1e-3 + + if self.opti_callback_save_costs is None: + self.opti_callback_save_costs = True + + if self.opti_callback_save_constraint_multipliers is None: + self.opti_callback_save_constraint_multipliers = True def is_valid(self) -> bool: number_of_joints = len(self.joints_name_list) From ca736065a3125b8a3d3164cc1f59e4c9db149a4a Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 26 Feb 2024 15:41:04 +0100 Subject: [PATCH 02/16] Improved the settings validation in the kinodynamic planner --- .../humanoid_kinodynamic/settings.py | 153 ++++++++++++++---- 1 file changed, 119 insertions(+), 34 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index 78d39d98..d0b7fa74 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -1,4 +1,5 @@ import dataclasses +import logging import typing import numpy as np @@ -143,38 +144,122 @@ def __post_init__(self) -> None: self.opti_callback_save_constraint_multipliers = True def is_valid(self) -> bool: + ok = True + logger = logging.getLogger("[hippopt::HumanoidKynodynamic::Settings]") number_of_joints = len(self.joints_name_list) - return ( - self.robot_urdf is not None - and self.joints_name_list is not None - and self.contact_points is not None - and self.horizon_length is not None - and self.time_step is not None - and self.minimum_com_height is not None - and self.minimum_feet_lateral_distance is not None - and self.maximum_feet_relative_height is not None - and self.maximum_joint_positions is not None - and self.minimum_joint_positions is not None - and self.maximum_joint_velocities is not None - and self.minimum_joint_velocities is not None - and len(self.maximum_joint_positions) == number_of_joints - and len(self.minimum_joint_positions) == number_of_joints - and len(self.maximum_joint_velocities) == number_of_joints - and len(self.minimum_joint_velocities) == number_of_joints - and self.contacts_centroid_cost_multiplier is not None - and self.com_linear_velocity_cost_weights is not None - and len(self.com_linear_velocity_cost_weights) == 3 - and self.com_linear_velocity_cost_multiplier is not None - and self.desired_frame_quaternion_cost_frame_name is not None - and self.desired_frame_quaternion_cost_multiplier is not None - and self.base_quaternion_cost_multiplier is not None - and self.base_quaternion_velocity_cost_multiplier is not None - and self.joint_regularization_cost_weights is not None - and len(self.joint_regularization_cost_weights) == number_of_joints - and self.joint_regularization_cost_multiplier is not None - and self.force_regularization_cost_multiplier is not None - and self.foot_yaw_regularization_cost_multiplier is not None - and self.swing_foot_height_cost_multiplier is not None - and self.contact_velocity_control_cost_multiplier is not None - and self.contact_force_control_cost_multiplier is not None - ) + if self.robot_urdf is None: + logger.error("robot_urdf is None") + ok = False + if self.joints_name_list is None: + logger.error("joints_name_list is None") + ok = False + if self.contact_points is None: + logger.error("contact_points is None") + ok = False + if self.horizon_length is None: + logger.error("horizon_length is None") + ok = False + if self.time_step is None: + logger.error("time_step is None") + ok = False + if self.minimum_com_height is None: + logger.error("minimum_com_height is None") + ok = False + if self.minimum_feet_lateral_distance is None: + logger.error("minimum_feet_lateral_distance is None") + ok = False + if self.maximum_feet_relative_height is None: + logger.error("maximum_feet_relative_height is None") + ok = False + if self.maximum_joint_positions is None: + logger.error("maximum_joint_positions is None") + ok = False + if self.minimum_joint_positions is None: + logger.error("minimum_joint_positions is None") + ok = False + if self.maximum_joint_velocities is None: + logger.error("maximum_joint_velocities is None") + ok = False + if self.minimum_joint_velocities is None: + logger.error("minimum_joint_velocities is None") + ok = False + if len(self.maximum_joint_positions) != number_of_joints: + logger.error( + f"len(maximum_joint_positions)={len(self.maximum_joint_positions)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if len(self.minimum_joint_positions) != number_of_joints: + logger.error( + f"len(minimum_joint_positions)={len(self.minimum_joint_positions)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if len(self.maximum_joint_velocities) != number_of_joints: + logger.error( + f"len(maximum_joint_velocities)={len(self.maximum_joint_velocities)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if len(self.minimum_joint_velocities) != number_of_joints: + logger.error( + f"len(minimum_joint_velocities)={len(self.minimum_joint_velocities)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if self.contacts_centroid_cost_multiplier is None: + logger.error("contacts_centroid_cost_multiplier is None") + ok = False + if self.com_linear_velocity_cost_weights is None: + logger.error("com_linear_velocity_cost_weights is None") + ok = False + if len(self.com_linear_velocity_cost_weights) != 3: + logger.error( + f"len(com_linear_velocity_cost_weights)=" + f"{len(self.com_linear_velocity_cost_weights)} != 3" + ) + ok = False + if self.com_linear_velocity_cost_multiplier is None: + logger.error("com_linear_velocity_cost_multiplier is None") + ok = False + if self.desired_frame_quaternion_cost_frame_name is None: + logger.error("desired_frame_quaternion_cost_frame_name is None") + ok = False + if self.desired_frame_quaternion_cost_multiplier is None: + logger.error("desired_frame_quaternion_cost_multiplier is None") + ok = False + if self.base_quaternion_cost_multiplier is None: + logger.error("base_quaternion_cost_multiplier is None") + ok = False + if self.base_quaternion_velocity_cost_multiplier is None: + logger.error("base_quaternion_velocity_cost_multiplier is None") + ok = False + if self.joint_regularization_cost_weights is None: + logger.error("joint_regularization_cost_weights is None") + ok = False + if len(self.joint_regularization_cost_weights) != number_of_joints: + logger.error( + f"len(joint_regularization_cost_weights)=" + f"{len(self.joint_regularization_cost_weights)} !=" + f" number_of_joints={number_of_joints}" + ) + ok = False + if self.joint_regularization_cost_multiplier is None: + logger.error("joint_regularization_cost_multiplier is None") + ok = False + if self.force_regularization_cost_multiplier is None: + logger.error("force_regularization_cost_multiplier is None") + ok = False + if self.foot_yaw_regularization_cost_multiplier is None: + logger.error("foot_yaw_regularization_cost_multiplier is None") + ok = False + if self.swing_foot_height_cost_multiplier is None: + logger.error("swing_foot_height_cost_multiplier is None") + ok = False + if self.contact_velocity_control_cost_multiplier is None: + logger.error("contact_velocity_control_cost_multiplier is None") + ok = False + if self.contact_force_control_cost_multiplier is None: + logger.error("contact_force_control_cost_multiplier is None") + ok = False + return ok From 17e7a7a58624e09e62240621ddf324e518682ae1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 26 Feb 2024 12:57:22 +0100 Subject: [PATCH 03/16] The kinematics expressions can accept a parametric kindyn object --- .../robot_planning/expressions/kinematics.py | 260 +++++++++++++++++- 1 file changed, 250 insertions(+), 10 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index bae777a4..c253dab6 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -1,6 +1,7 @@ import casadi as cs import liecasadi from adam.casadi import KinDynComputations +from adam.parametric.casadi import KinDynComputationsParametric from hippopt.robot_planning.expressions.quaternion import ( quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, @@ -8,13 +9,15 @@ def centroidal_momentum_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", base_position_derivative_name: str = "base_position_derivative", base_quaternion_xyzw_derivative_name: str = "base_quaternion_derivative", joint_velocities_name: str = "joint_velocities", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -27,6 +30,20 @@ def centroidal_momentum_from_kinematics( base_quaternion_derivative = cs.MX.sym(base_quaternion_xyzw_derivative_name, 4) joint_velocities = cs.MX.sym(joint_velocities_name, kindyn_object.NDoF) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + cmm_function = kindyn_object.centroidal_momentum_matrix_fun() base_pose = liecasadi.SE3.from_position_quaternion( @@ -51,6 +68,43 @@ def centroidal_momentum_from_kinematics( base_position_derivative, base_angular_velocity, joint_velocities ) + if parametric: + momentum = ( + cmm_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + @ robot_velocity + ) + return cs.Function( + "centroidal_momentum_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + base_position_derivative, + base_quaternion_derivative, + joint_velocities, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [momentum], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + base_position_derivative_name, + base_quaternion_xyzw_derivative_name, + joint_velocities_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["h_g"], + options, + ) + momentum = cmm_function(base_pose, joint_positions) @ robot_velocity return cs.Function( @@ -78,10 +132,12 @@ def centroidal_momentum_from_kinematics( def center_of_mass_position_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -91,12 +147,53 @@ def center_of_mass_position_from_kinematics( base_quaternion = cs.MX.sym(base_quaternion_xyzw_name, 4) joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + com_function = kindyn_object.CoM_position_fun() base_pose = liecasadi.SE3.from_position_quaternion( base_position, base_quaternion ).as_matrix() # The quaternion is supposed normalized + if parametric: + com_position = com_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + return cs.Function( + "center_of_mass_position_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [com_position], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["com_position"], + options, + ) + com_position = com_function(base_pose, joint_positions) return cs.Function( @@ -118,12 +215,14 @@ def center_of_mass_position_from_kinematics( def point_position_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, frame_name: str, point_position_in_frame_name: str = "point_position", base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -134,16 +233,61 @@ def point_position_from_kinematics( joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) point_position_in_frame = cs.MX.sym(point_position_in_frame_name, 3) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + "parametric_links_length_multiplier", + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + "parametric_links_densities", kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + fk_function = kindyn_object.forward_kinematics_fun(frame=frame_name) base_pose = liecasadi.SE3.from_position_quaternion( base_position, base_quaternion ).as_matrix() # The quaternion is supposed normalized - frame_pose = fk_function(base_pose, joint_positions) + if parametric: + frame_pose = fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + else: + frame_pose = fk_function(base_pose, joint_positions) point_position = frame_pose[:3, :3] @ point_position_in_frame + frame_pose[:3, 3] + if parametric: + return cs.Function( + "point_position_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + point_position_in_frame, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [point_position], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + point_position_in_frame_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["point_position"], + options, + ) + return cs.Function( "point_position_from_kinematics", [ @@ -165,34 +309,81 @@ def point_position_from_kinematics( def frames_relative_position( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, reference_frame: str, target_frame: str, joint_positions_name: str = "joint_positions", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: options = {} if options is None else options joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + base_pose = cs.DM_eye(4) reference_fk_function = kindyn_object.forward_kinematics_fun(frame=reference_frame) target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) - reference_pose = reference_fk_function(base_pose, joint_positions) + if parametric: + reference_pose = reference_fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + target_pose = target_fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + else: + reference_pose = reference_fk_function(base_pose, joint_positions) + target_pose = target_fk_function(base_pose, joint_positions) + reference_pose_inverse_rotation = reference_pose[:3, :3].T reference_pose_inverse_translation = ( -reference_pose_inverse_rotation @ reference_pose[:3, 3] ) - target_pose = target_fk_function(base_pose, joint_positions) - relative_position = ( reference_pose_inverse_rotation @ target_pose[:3, 3] + reference_pose_inverse_translation ) + if parametric: + return cs.Function( + "frames_relative_position_parametric", + [ + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [relative_position], + [ + joint_positions_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["relative_position"], + options, + ) + return cs.Function( "frames_relative_position", [joint_positions], @@ -204,12 +395,14 @@ def frames_relative_position( def rotation_error_from_kinematics( - kindyn_object: KinDynComputations, + kindyn_object: KinDynComputations | KinDynComputationsParametric, target_frame: str, base_position_name: str = "base_position", base_quaternion_xyzw_name: str = "base_quaternion", joint_positions_name: str = "joint_positions", desired_quaternion_xyzw_name: str = "desired_quaternion", + parametric_link_length_multipliers_name: str = "parametric_link_length_multipliers", + parametric_link_densities_name: str = "parametric_link_densities", options: dict = None, **_ ) -> cs.Function: @@ -219,18 +412,65 @@ def rotation_error_from_kinematics( joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) desired_quaternion = cs.MX.sym(desired_quaternion_xyzw_name, 4) + parametric = isinstance(kindyn_object, KinDynComputationsParametric) + if parametric: + parametric_links_length_multiplier = cs.MX.sym( + parametric_link_length_multipliers_name, + kindyn_object.length_multiplier.shape[0], + ) + parametric_links_densities = cs.MX.sym( + parametric_link_densities_name, kindyn_object.densities.shape[0] + ) + else: + parametric_links_length_multiplier = None + parametric_links_densities = None + base_pose = liecasadi.SE3.from_position_quaternion( base_position, base_quaternion ).as_matrix() # The quaternion is supposed normalized target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) - target_pose = target_fk_function(base_pose, joint_positions) + + if parametric: + target_pose = target_fk_function( + base_pose, + joint_positions, + parametric_links_length_multiplier, + parametric_links_densities, + ) + else: + target_pose = target_fk_function(base_pose, joint_positions) + target_orientation = target_pose[:3, :3] rotation_error = ( target_orientation @ liecasadi.SO3.from_quat(desired_quaternion).as_matrix().T ) + if parametric: + return cs.Function( + "rotation_error_from_kinematics_parametric", + [ + base_position, + base_quaternion, + joint_positions, + desired_quaternion, + parametric_links_length_multiplier, + parametric_links_densities, + ], + [rotation_error], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + desired_quaternion_xyzw_name, + parametric_link_length_multipliers_name, + parametric_link_densities_name, + ], + ["rotation_error"], + options, + ) + return cs.Function( "quaternion_error_from_kinematics", [ From 1e9919c58e936082e2879579ed78ee92dd4f3111 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 27 Feb 2024 19:09:02 +0100 Subject: [PATCH 04/16] Added modifications to use a parametric model in the humanoid_kinodynamic planner --- .../humanoid_kinodynamic/planner.py | 71 +++++++++++++------ .../humanoid_kinodynamic/settings.py | 11 +++ .../humanoid_kinodynamic/variables.py | 41 +++++++++-- 3 files changed, 98 insertions(+), 25 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index d72db04e..a0c48c0d 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -1,6 +1,7 @@ import copy import adam.casadi +import adam.parametric.casadi import casadi as cs import numpy as np @@ -21,14 +22,26 @@ def __init__(self, settings: Settings) -> None: if not settings.is_valid(): raise ValueError("Settings are not valid") self.settings = copy.deepcopy(settings) - self.kin_dyn_object = adam.casadi.KinDynComputations( - urdfstring=self.settings.robot_urdf, - joints_name_list=self.settings.joints_name_list, - root_link=self.settings.root_link, - gravity=self.settings.gravity, - f_opts=self.settings.casadi_function_options, - ) - self.numeric_mass = self.kin_dyn_object.get_total_mass() + if self.settings.parametric_link_names is not None: + self.parametric = True + self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + links_name_list=self.settings.parametric_link_names, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + else: + self.parametric = False + self.kin_dyn_object = adam.casadi.KinDynComputations( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + self.numeric_mass = self.kin_dyn_object.get_total_mass() self.variables = Variables( settings=self.settings, kin_dyn_object=self.kin_dyn_object @@ -858,7 +871,16 @@ def _add_periodicity_expression(self, all_contact_points): ) def _apply_mass_regularization(self, input_var: Variables) -> Variables: - assert self.numeric_mass > 0 + if self.parametric: + numeric_mass_fun = self.kin_dyn_object.get_total_mass() + numeric_mass = numeric_mass_fun( # noqa It is a function, not a float + input_var.parametric_link_densities, # WARNING the order might change + input_var.parametric_link_length_multipliers, + ) + else: + numeric_mass = self.numeric_mass + + assert numeric_mass > 0 output = input_var if output.initial_state is not None: if ( @@ -866,19 +888,19 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: and len(output.initial_state.centroidal_momentum.shape) > 0 and output.initial_state.centroidal_momentum.shape[0] == 6 ): - output.initial_state.centroidal_momentum /= self.numeric_mass + output.initial_state.centroidal_momentum /= numeric_mass for point in ( output.initial_state.contact_points.left + output.initial_state.contact_points.right ): - point.f /= self.numeric_mass + point.f /= numeric_mass if output.final_state is not None: for point in ( output.final_state.contact_points.left + output.final_state.contact_points.right ): - point.f /= self.numeric_mass + point.f /= numeric_mass if output.system is None: return output @@ -889,14 +911,23 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: for system in system_list: if system.centroidal_momentum is not None: - system.centroidal_momentum /= self.numeric_mass + system.centroidal_momentum /= numeric_mass for point in system.contact_points.left + system.contact_points.right: - point.f /= self.numeric_mass + point.f /= numeric_mass return output def _undo_mass_regularization(self, input_var: Variables) -> Variables: - assert self.numeric_mass > 0 + if self.parametric: + numeric_mass_fun = self.kin_dyn_object.get_total_mass() + numeric_mass = numeric_mass_fun( # noqa It is a function, not a float + input_var.parametric_link_densities, # WARNING the order might change + input_var.parametric_link_length_multipliers, + ) + else: + numeric_mass = self.numeric_mass + + assert numeric_mass > 0 output = input_var if output.initial_state is not None: if ( @@ -904,19 +935,19 @@ def _undo_mass_regularization(self, input_var: Variables) -> Variables: and len(output.initial_state.centroidal_momentum.shape) > 0 and output.initial_state.centroidal_momentum.shape[0] == 6 ): - output.initial_state.centroidal_momentum *= self.numeric_mass + output.initial_state.centroidal_momentum *= numeric_mass for point in ( output.initial_state.contact_points.left + output.initial_state.contact_points.right ): - point.f *= self.numeric_mass + point.f *= numeric_mass if output.final_state is not None: for point in ( output.final_state.contact_points.left + output.final_state.contact_points.right ): - point.f *= self.numeric_mass + point.f *= numeric_mass if output.system is None: return output @@ -927,9 +958,9 @@ def _undo_mass_regularization(self, input_var: Variables) -> Variables: for system in system_list: if system.centroidal_momentum is not None: - system.centroidal_momentum *= self.numeric_mass + system.centroidal_momentum *= numeric_mass for point in system.contact_points.left + system.contact_points.right: - point.f *= self.numeric_mass + point.f *= numeric_mass return output diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index d0b7fa74..f6e6b103 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -13,6 +13,10 @@ class Settings: robot_urdf: str = dataclasses.field(default=None) joints_name_list: list[str] = dataclasses.field(default=None) + parametric_link_names: list[str] = dataclasses.field(default=None) + initial_densities: np.ndarray = dataclasses.field( + default=None + ) # Necessary because of https://github.com/ami-iit/adam/issues/70 contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) gravity: np.array = dataclasses.field(default=None) @@ -153,6 +157,13 @@ def is_valid(self) -> bool: if self.joints_name_list is None: logger.error("joints_name_list is None") ok = False + if self.parametric_link_names is not None: + if len(self.parametric_link_names) != len(self.initial_densities): + logger.error( + f"len(parametric_link_names)={len(self.parametric_link_names)} !=" + f" len(initial_densities)={len(self.initial_densities)}" + ) + ok = False if self.contact_points is None: logger.error("contact_points is None") ok = False diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index a0978f61..fddc8be8 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -1,7 +1,9 @@ +import copy import dataclasses from typing import TypeVar import adam.casadi +import adam.parametric.casadi import numpy as np import hippopt as hp @@ -259,6 +261,11 @@ class Variables(hp.OptimizationObject): mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + parametric_link_length_multipliers: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + parametric_link_densities: hp.StorageType = hp.default_storage_field(hp.Parameter) + initial_state: hp.CompositeType[ExtendedHumanoidState] = hp.default_composite_field( cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False ) @@ -295,14 +302,18 @@ class Variables(hp.OptimizationObject): ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) - kin_dyn_object: dataclasses.InitVar[adam.casadi.KinDynComputations] = ( - dataclasses.field(default=None) - ) + kin_dyn_object: dataclasses.InitVar[ + adam.casadi.KinDynComputations + | adam.parametric.casadi.KinDynComputationsParametric + ] = dataclasses.field(default=None) def __post_init__( self, settings: Settings, - kin_dyn_object: adam.casadi.KinDynComputations, + kin_dyn_object: ( + adam.casadi.KinDynComputations + | adam.parametric.casadi.KinDynComputationsParametric + ), ) -> None: self.system = ExtendedHumanoid( contact_point_descriptors=settings.contact_points, @@ -321,7 +332,27 @@ def __post_init__( self.dt = settings.time_step self.gravity = kin_dyn_object.g - self.mass = kin_dyn_object.get_total_mass() + + self.parametric_link_length_multipliers = ( + np.ones(len(settings.parametric_link_names)) + if settings.parametric_link_names is not None + else np.array([]) + ) + self.parametric_link_densities = ( + copy.deepcopy(settings.initial_densities) + if settings.initial_densities is not None + else np.array([]) + ) + + if isinstance( + kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ): + total_mass_fun = kin_dyn_object.get_total_mass() + self.mass = total_mass_fun( # noqa + self.parametric_link_densities, self.parametric_link_length_multipliers + ) # WARNING, the order might change + else: + self.mass = kin_dyn_object.get_total_mass() self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier self.dcc_gain = settings.dcc_gain From 3724f79d5fe1e3871183395c9c7a62c06aa8dca7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 29 Feb 2024 13:03:04 +0100 Subject: [PATCH 05/16] Added main for periodic step with stickbot --- .../main_periodic_step_parametric.py | 498 ++++++++++++++++++ 1 file changed, 498 insertions(+) create mode 100644 src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py new file mode 100644 index 00000000..12e0789f --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.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://stickBot/model.urdf" + ) + settings = walking_settings.Settings() + settings.robot_urdf = str(urdf_path) + settings.joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", + ] + number_of_joints = len(settings.joints_name_list) + idyntree_model_loader = idyntree.ModelLoader() + idyntree_model_loader.loadReducedModelFromFile( + settings.robot_urdf, settings.joints_name_list + ) + idyntree_model = idyntree_model_loader.model() + settings.root_link = "root_link" + settings.horizon_length = 30 + settings.time_step = 0.1 + settings.contact_points = hp_rp.FeetContactPointDescriptors() + settings.contact_points.left = hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="l_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.05, 0.0]), + ) + settings.contact_points.right = hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="r_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.05, 0.0]), + ) + settings.planar_dcc_height_multiplier = 10.0 + settings.dcc_gain = 40.0 + settings.dcc_epsilon = 0.005 + settings.static_friction = 0.3 + settings.maximum_velocity_control = [2.0, 2.0, 5.0] + settings.maximum_force_derivative = [500.0, 500.0, 500.0] + settings.maximum_angular_momentum = 5.0 + settings.minimum_com_height = 0.3 + settings.minimum_feet_lateral_distance = 0.1 + settings.maximum_feet_relative_height = 0.05 + settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) + settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) + for i in range(number_of_joints): + joint = idyntree_model.getJoint(i) + if joint.hasPosLimits(): + settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) + settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) + settings.maximum_joint_velocities = np.ones(number_of_joints) * 2.0 + settings.minimum_joint_velocities = np.ones(number_of_joints) * -2.0 + settings.joint_regularization_cost_weights = np.ones(number_of_joints) + settings.joint_regularization_cost_weights[:3] = 0.1 # torso + settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + settings.joint_regularization_cost_weights[11:] = 1.0 # legs + settings.contacts_centroid_cost_multiplier = 0.0 + settings.com_linear_velocity_cost_weights = [10.0, 0.1, 1.0] + settings.com_linear_velocity_cost_multiplier = 1.0 + settings.desired_frame_quaternion_cost_frame_name = "chest" + settings.desired_frame_quaternion_cost_multiplier = 200.0 + settings.base_quaternion_cost_multiplier = 50.0 + settings.base_quaternion_velocity_cost_multiplier = 0.001 + settings.joint_regularization_cost_multiplier = 10.0 + settings.force_regularization_cost_multiplier = 10.0 + settings.foot_yaw_regularization_cost_multiplier = 2000.0 + settings.swing_foot_height_cost_multiplier = 1000.0 + settings.contact_velocity_control_cost_multiplier = 5.0 + settings.contact_force_control_cost_multiplier = 0.0001 + settings.final_state_expression_type = hippopt.ExpressionType.subject_to + settings.periodicity_expression_type = hippopt.ExpressionType.subject_to + settings.casadi_function_options = {"cse": True} + settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True} + settings.casadi_solver_options = { + "max_iter": 4000, + "linear_solver": "mumps", + "alpha_for_y": "dual-and-full", + "fast_step_computation": "yes", + "hessian_approximation": "limited-memory", + "tol": 1e-3, + "dual_inf_tol": 1000.0, + "compl_inf_tol": 1e-2, + "constr_viol_tol": 1e-4, + "acceptable_tol": 10, + "acceptable_iter": 2, + "acceptable_compl_inf_tol": 1000.0, + "warm_start_bound_frac": 1e-2, + "warm_start_bound_push": 1e-2, + "warm_start_mult_bound_push": 1e-2, + "warm_start_slack_bound_frac": 1e-2, + "warm_start_slack_bound_push": 1e-2, + "warm_start_init_point": "yes", + "required_infeasibility_reduction": 0.8, + "perturb_dec_fact": 0.1, + "max_hessian_perturbation": 100.0, + "acceptable_obj_change_tol": 1e0, + } + + return settings + + +def get_pose_finder_settings( + input_settings: walking_settings.Settings, +) -> pose_finder.Settings: + number_of_joints = len(input_settings.joints_name_list) + settings = pose_finder.Settings() + settings.robot_urdf = input_settings.robot_urdf + settings.joints_name_list = input_settings.joints_name_list + + settings.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_parametric_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_parametric", + ) From 9f57742d88eb73669930f62f8ebae45bdc061cee Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 29 Feb 2024 18:52:44 +0100 Subject: [PATCH 06/16] Inverting the inputs to get_total_mass This needs https://github.com/ami-iit/adam/pull/72 to be merged --- .../humanoid_kinodynamic/planner.py | 14 ++++++++++---- .../humanoid_kinodynamic/variables.py | 6 +++--- 2 files changed, 13 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index a0c48c0d..bdd63afa 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -872,10 +872,13 @@ def _add_periodicity_expression(self, all_contact_points): def _apply_mass_regularization(self, input_var: Variables) -> Variables: if self.parametric: + assert isinstance( + self.kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ) numeric_mass_fun = self.kin_dyn_object.get_total_mass() - numeric_mass = numeric_mass_fun( # noqa It is a function, not a float - input_var.parametric_link_densities, # WARNING the order might change + numeric_mass = numeric_mass_fun( input_var.parametric_link_length_multipliers, + input_var.parametric_link_densities, ) else: numeric_mass = self.numeric_mass @@ -919,10 +922,13 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: def _undo_mass_regularization(self, input_var: Variables) -> Variables: if self.parametric: + assert isinstance( + self.kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ) numeric_mass_fun = self.kin_dyn_object.get_total_mass() - numeric_mass = numeric_mass_fun( # noqa It is a function, not a float - input_var.parametric_link_densities, # WARNING the order might change + numeric_mass = numeric_mass_fun( input_var.parametric_link_length_multipliers, + input_var.parametric_link_densities, ) else: numeric_mass = self.numeric_mass diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index fddc8be8..8ee1dbcd 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -348,9 +348,9 @@ def __post_init__( kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric ): total_mass_fun = kin_dyn_object.get_total_mass() - self.mass = total_mass_fun( # noqa - self.parametric_link_densities, self.parametric_link_length_multipliers - ) # WARNING, the order might change + self.mass = total_mass_fun( + self.parametric_link_length_multipliers, self.parametric_link_densities + ) else: self.mass = kin_dyn_object.get_total_mass() From da196e6a56277d9cfb58d504e031b80aec3c10e0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 29 Feb 2024 18:54:40 +0100 Subject: [PATCH 07/16] Dealing with the case where am optimization object is an empty list. --- src/hippopt/base/opti_solver.py | 18 +++++++++++++----- 1 file changed, 13 insertions(+), 5 deletions(-) diff --git a/src/hippopt/base/opti_solver.py b/src/hippopt/base/opti_solver.py index 47d820ff..6d7c31dd 100644 --- a/src/hippopt/base/opti_solver.py +++ b/src/hippopt/base/opti_solver.py @@ -152,6 +152,9 @@ def _generate_opti_object( if isinstance(value, float): value = value * np.ones((1, 1)) + if value.shape[0] * value.shape[1] == 0: + raise ValueError("Field " + name + " has a zero dimension.") + if storage_type is Variable.StorageTypeValue: self._logger.debug("Creating variable " + name) opti_object = self._solver.variable(*value.shape) @@ -181,12 +184,17 @@ def _generate_objects_from_instance( composite_value = output.__getattribute__(field.name) is_list = isinstance(composite_value, list) - list_of_optimization_objects = is_list and all( - isinstance(elem, OptimizationObject) or isinstance(elem, list) - for elem in composite_value + list_of_optimization_objects = ( + is_list + and len(composite_value) > 0 + and all( + isinstance(elem, OptimizationObject) or isinstance(elem, list) + for elem in composite_value + ) ) - list_of_float = is_list and all( - isinstance(elem, float) for elem in composite_value + list_of_float = is_list and ( + len(composite_value) == 0 + or all(isinstance(elem, float) for elem in composite_value) ) if list_of_float: composite_value = np.array(composite_value) From 1bdb8efc3289096d982c29c8bf5e1a4a15a7961f Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 4 Mar 2024 15:54:00 +0100 Subject: [PATCH 08/16] Fixed the calling of the parametric version of the kinematic functions in the kinodynamic planner --- .../humanoid_kinodynamic/planner.py | 114 +++++++++++++----- .../humanoid_kinodynamic/variables.py | 7 +- 2 files changed, 87 insertions(+), 34 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index bdd63afa..98cdbc56 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -23,7 +23,7 @@ def __init__(self, settings: Settings) -> None: raise ValueError("Settings are not valid") self.settings = copy.deepcopy(settings) if self.settings.parametric_link_names is not None: - self.parametric = True + self.parametric_model = True self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric( urdfstring=self.settings.robot_urdf, joints_name_list=self.settings.joints_name_list, @@ -33,7 +33,7 @@ def __init__(self, settings: Settings) -> None: f_opts=self.settings.casadi_function_options, ) else: - self.parametric = False + self.parametric_model = False self.kin_dyn_object = adam.casadi.KinDynComputations( urdfstring=self.settings.robot_urdf, joints_name_list=self.settings.joints_name_list, @@ -197,6 +197,8 @@ def _get_function_inputs_dict(self): "second_point_name": "p_1", "desired_yaw_name": "yd", "desired_height_name": "hd", + "parametric_link_length_multipliers_name": "pi_l", + "parametric_link_densities_name": "pi_d", "options": self.settings.casadi_function_options, } return function_inputs @@ -274,11 +276,20 @@ def _add_kinematics_constraints( com_kinematics_fun = hp_rp.center_of_mass_position_from_kinematics( kindyn_object=self.kin_dyn_object, **function_inputs ) - com_kinematics = com_kinematics_fun( - pb=sym.system.kinematics.base.position, - qb=normalized_quaternion, - s=sym.system.kinematics.joints.positions, - )["com_position"] + if self.parametric_model: + com_kinematics = com_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + )["com_position"] + else: + com_kinematics = com_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + )["com_position"] problem.add_expression_to_horizon( expression=cs.MX(sym.system.com == com_kinematics), apply_to_first_elements=False, @@ -289,14 +300,26 @@ def _add_kinematics_constraints( centroidal_kinematics_fun = hp_rp.centroidal_momentum_from_kinematics( kindyn_object=self.kin_dyn_object, **function_inputs ) - centroidal_kinematics = centroidal_kinematics_fun( - pb=sym.system.kinematics.base.position, - qb=normalized_quaternion, - s=sym.system.kinematics.joints.positions, - pb_dot=sym.system.kinematics.base.linear_velocity, - qb_dot=sym.system.kinematics.base.quaternion_velocity_xyzw, - s_dot=sym.system.kinematics.joints.velocities, - )["h_g"] + if self.parametric_model: + centroidal_kinematics = centroidal_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + pb_dot=sym.system.kinematics.base.linear_velocity, + qb_dot=sym.system.kinematics.base.quaternion_velocity_xyzw, + s_dot=sym.system.kinematics.joints.velocities, + )["h_g"] + else: + centroidal_kinematics = centroidal_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pb_dot=sym.system.kinematics.base.linear_velocity, + qb_dot=sym.system.kinematics.base.quaternion_velocity_xyzw, + s_dot=sym.system.kinematics.joints.velocities, + )["h_g"] problem.add_expression_to_horizon( expression=cs.MX( sym.system.centroidal_momentum[3:] @@ -334,9 +357,16 @@ def _add_kinematics_constraints( target_frame=left_frame, **function_inputs, ) - relative_position = relative_position_fun( - s=sym.system.kinematics.joints.positions - )["relative_position"] + if self.parametric_model: + relative_position = relative_position_fun( + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + )["relative_position"] + else: + relative_position = relative_position_fun( + s=sym.system.kinematics.joints.positions + )["relative_position"] problem.add_expression_to_horizon( expression=cs.MX(relative_position[1] >= sym.minimum_feet_lateral_distance), apply_to_first_elements=False, @@ -413,12 +443,22 @@ def _add_kinematics_regularization( target_frame=self.settings.desired_frame_quaternion_cost_frame_name, **function_inputs, ) - rotation_error_kinematics = rotation_error_kinematics_fun( - pb=sym.system.kinematics.base.position, - qb=base_quaternion_normalized, - s=sym.system.kinematics.joints.positions, - qd=sym.references.desired_frame_quaternion_xyzw, - )["rotation_error"] + if self.parametric_model: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=base_quaternion_normalized, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + qd=sym.references.desired_frame_quaternion_xyzw, + )["rotation_error"] + else: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=sym.system.kinematics.base.position, + qb=base_quaternion_normalized, + s=sym.system.kinematics.joints.positions, + qd=sym.references.desired_frame_quaternion_xyzw, + )["rotation_error"] problem.add_expression_to_horizon( expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), apply_to_first_elements=False, @@ -560,12 +600,22 @@ def _add_contact_kinematic_consistency( ) # Consistency between the contact position and the kinematics - point_kinematics = point_kinematics_functions[descriptor.foot_frame]( - pb=sym.system.kinematics.base.position, - qb=normalized_quaternion, - s=sym.system.kinematics.joints.positions, - p_parent=descriptor.position_in_foot_frame, - )["point_position"] + if self.parametric_model: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + pi_l=sym.parametric_link_length_multipliers, + pi_d=sym.parametric_link_densities, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] + else: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=sym.system.kinematics.base.position, + qb=normalized_quaternion, + s=sym.system.kinematics.joints.positions, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] problem.add_expression_to_horizon( expression=cs.MX(point.p == point_kinematics), apply_to_first_elements=False, @@ -871,7 +921,7 @@ def _add_periodicity_expression(self, all_contact_points): ) def _apply_mass_regularization(self, input_var: Variables) -> Variables: - if self.parametric: + if self.parametric_model: assert isinstance( self.kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric ) @@ -921,7 +971,7 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: return output def _undo_mass_regularization(self, input_var: Variables) -> Variables: - if self.parametric: + if self.parametric_model: assert isinstance( self.kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric ) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index 8ee1dbcd..18008341 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -348,8 +348,11 @@ def __post_init__( kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric ): total_mass_fun = kin_dyn_object.get_total_mass() - self.mass = total_mass_fun( - self.parametric_link_length_multipliers, self.parametric_link_densities + self.mass = float( + total_mass_fun( + self.parametric_link_length_multipliers, + self.parametric_link_densities, + ).full() ) else: self.mass = kin_dyn_object.get_total_mass() From 4dbe398fb889bb679f6aa691c1a826195483716b Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 4 Mar 2024 16:01:12 +0100 Subject: [PATCH 09/16] Specify the list of parametric links in main_periodic_step_parametric --- .../main_periodic_step_parametric.py | 39 ++++++++++++------- 1 file changed, 26 insertions(+), 13 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py index 12e0789f..6ce323de 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py @@ -15,6 +15,9 @@ def get_planner_settings() -> walking_settings.Settings: + # The model is available at + # https://github.com/icub-tech-iit/ergocub-gazebo-simulations/tree/↵ + # 1179630a88541479df51ebb108a21865ea251302/models/stickBot urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( "package://stickBot/model.urdf" ) @@ -45,6 +48,26 @@ def get_planner_settings() -> walking_settings.Settings: "r_ankle_pitch", "r_ankle_roll", ] + settings.parametric_link_names = [ + "r_upper_arm", + "r_forearm", + "l_hip_3", + "l_lower_leg", + "root_link", + "torso_1", + "torso_2", + "chest", + ] + settings.initial_densities = [ + 1661.6863265236248, + 727.4313078156689, + 600.8642717368293, + 2134.3111071426842, + 2129.295296396375, + 1199.0762240824756, + 893.1076351798705, + 626.6027187152905, + ] number_of_joints = len(settings.joints_name_list) idyntree_model_loader = idyntree.ModelLoader() idyntree_model_loader.loadReducedModelFromFile( @@ -452,19 +475,6 @@ def get_references( 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_parametric_guess", - ) - print("Starting the planner...") references = get_references( @@ -486,6 +496,9 @@ def get_references( humanoid_states = [s.to_humanoid_state() for s in output.values.system] left_contact_points = [s.contact_points.left for s in humanoid_states] right_contact_points = [s.contact_points.right for s in humanoid_states] + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) print("Press [Enter] to visualize the solution.") input() From 6d81966623986d7c78be949d4eb1ab93348ddd37 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 14:34:39 +0100 Subject: [PATCH 10/16] Visualizing directly the model from adam instead of loading the file --- .../utilities/humanoid_state_visualizer.py | 27 ++++++++++++++----- .../main_periodic_step_parametric.py | 1 + .../humanoid_kinodynamic/planner.py | 24 +++++++++++++++++ 3 files changed, 45 insertions(+), 7 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index f5086878..05f136bb 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -4,9 +4,11 @@ import pathlib import time +import adam.model import ffmpeg import liecasadi import numpy as np +from adam.model.conversions import to_idyntree_model from idyntree.visualize import MeshcatVisualizer from hippopt.robot_planning.utilities.terrain_visualizer import ( @@ -21,7 +23,7 @@ @dataclasses.dataclass class HumanoidStateVisualizerSettings(TerrainVisualizerSettings): - robot_model: str = dataclasses.field(default=None) + robot_model: str | adam.model.Model = dataclasses.field(default=None) considered_joints: list[str] = dataclasses.field(default=None) contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) robot_color: list[float] = dataclasses.field(default=None) @@ -122,12 +124,23 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: self._set_clone_visibility(i, False) def _allocate_clone(self, index: int) -> None: - self._viz.load_model_from_file( - model_path=self._settings.robot_model, - model_name=f"[{index}]robot", - considered_joints=self._settings.considered_joints, - color=self._settings.robot_color, - ) + + if isinstance(self._settings.robot_model, str): + self._viz.load_model_from_file( + model_path=self._settings.robot_model, + model_name=f"[{index}]robot", + considered_joints=self._settings.considered_joints, + color=self._settings.robot_color, + ) + else: + idyntree_model = adam.model.conversions.to_idyntree_model( + self._settings.robot_model + ) + self._viz.load_model( + model=idyntree_model, + model_name=f"[{index}]robot", + color=self._settings.robot_color, + ) self._viz.load_sphere( radius=self._settings.com_radius, shape_name=f"[{index}]CoM", diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py index 6ce323de..e8a280b4 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py @@ -498,6 +498,7 @@ def get_references( right_contact_points = [s.contact_points.right for s in humanoid_states] visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer_settings.robot_model = planner.get_adam_model() visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) print("Press [Enter] to visualize the solution.") input() diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 98cdbc56..d1f2aeb5 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -1,9 +1,14 @@ import copy import adam.casadi +import adam.model +import adam.numpy import adam.parametric.casadi import casadi as cs import numpy as np +from adam.parametric.model.parametric_factories.parametric_model import ( + URDFParametricModelFactory, +) import hippopt as hp import hippopt.robot_planning as hp_rp @@ -1057,3 +1062,22 @@ def solve(self) -> hp.Output[Variables]: output = self.ocp.problem.solve() output.values = self._undo_mass_regularization(output.values) return output + + def get_adam_model(self) -> adam.model.Model: + if self.parametric_model: + guess = self.ocp.problem.get_initial_guess() + original_length = guess.parametric_link_length_multipliers + original_density = guess.parametric_link_densities + factory = URDFParametricModelFactory( + path=self.settings.robot_urdf, + math=adam.numpy.numpy_like.SpatialMath(), + links_name_list=self.settings.parametric_link_names, + length_multiplier=original_length, + densities=original_density, + ) + model = adam.model.Model.build( + factory=factory, joints_name_list=self.settings.joints_name_list + ) + return model + + return self.kin_dyn_object.rbdalgos.model From c1d51534e22a07465c1e14d04482e39782504d98 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 15:06:14 +0100 Subject: [PATCH 11/16] The link length multipliers and the densities are set to zero when not used --- .../turnkey_planners/humanoid_kinodynamic/variables.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index 18008341..46387abe 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -336,12 +336,12 @@ def __post_init__( self.parametric_link_length_multipliers = ( np.ones(len(settings.parametric_link_names)) if settings.parametric_link_names is not None - else np.array([]) + else 0.0 ) self.parametric_link_densities = ( copy.deepcopy(settings.initial_densities) if settings.initial_densities is not None - else np.array([]) + else 0.0 ) if isinstance( From a296cfecfec0a168384b920669aeb45a5af34d09 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 16:15:17 +0100 Subject: [PATCH 12/16] Using the adam model for visualization in the pose finder --- src/hippopt/turnkey_planners/humanoid_pose_finder/main.py | 2 +- src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py | 4 ++++ 2 files changed, 5 insertions(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index b3548abd..f30d5f15 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -166,7 +166,7 @@ output = planner.solve() visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() - visualizer_settings.robot_model = planner_settings.robot_urdf + visualizer_settings.robot_model = planner.get_adam_model() visualizer_settings.considered_joints = planner_settings.joints_name_list visualizer_settings.contact_points = planner_settings.contact_points visualizer_settings.terrain = planner_settings.terrain diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index b6cdd972..995e5f56 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -3,6 +3,7 @@ import logging import adam.casadi +import adam.model import casadi as cs import numpy as np @@ -572,3 +573,6 @@ def set_references(self, references: References) -> None: def solve(self) -> hp.Output[Variables]: return self.op.problem.solve() + + def get_adam_model(self) -> adam.model.Model: + return self.kin_dyn_object.rbdalgos.model From a397437badb8f9b3e9bb431d56432965e759c4d6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 8 Mar 2024 16:17:08 +0100 Subject: [PATCH 13/16] Getting the original densities directly from adam --- .../main_periodic_step_parametric.py | 31 +++++++++++++------ .../humanoid_kinodynamic/settings.py | 10 ------ .../humanoid_kinodynamic/variables.py | 9 ++---- 3 files changed, 24 insertions(+), 26 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py index e8a280b4..af92bc1b 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py @@ -58,16 +58,27 @@ def get_planner_settings() -> walking_settings.Settings: "torso_2", "chest", ] - settings.initial_densities = [ - 1661.6863265236248, - 727.4313078156689, - 600.8642717368293, - 2134.3111071426842, - 2129.295296396375, - 1199.0762240824756, - 893.1076351798705, - 626.6027187152905, - ] + # settings.parametric_link_densities = [ + # 1661.6863265236248, + # 727.4313078156689, + # 600.8642717368293, + # 2134.3111071426842, + # 2129.295296396375, + # 1199.0762240824756, + # 893.1076351798705, + # 626.6027187152905, + # ] + # settings.parametric_link_length_multipliers = [ + # 1.0, + # 1.0, + # 1.0, + # 1.0, + # 1.0, + # 1.0, + # 1.0, + # 1.0, + # ] + number_of_joints = len(settings.joints_name_list) idyntree_model_loader = idyntree.ModelLoader() idyntree_model_loader.loadReducedModelFromFile( diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index f6e6b103..d3e44745 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -14,9 +14,6 @@ class Settings: robot_urdf: str = dataclasses.field(default=None) joints_name_list: list[str] = dataclasses.field(default=None) parametric_link_names: list[str] = dataclasses.field(default=None) - initial_densities: np.ndarray = dataclasses.field( - default=None - ) # Necessary because of https://github.com/ami-iit/adam/issues/70 contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) gravity: np.array = dataclasses.field(default=None) @@ -157,13 +154,6 @@ def is_valid(self) -> bool: if self.joints_name_list is None: logger.error("joints_name_list is None") ok = False - if self.parametric_link_names is not None: - if len(self.parametric_link_names) != len(self.initial_densities): - logger.error( - f"len(parametric_link_names)={len(self.parametric_link_names)} !=" - f" len(initial_densities)={len(self.initial_densities)}" - ) - ok = False if self.contact_points is None: logger.error("contact_points is None") ok = False diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index 46387abe..7c692d72 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -1,4 +1,3 @@ -import copy import dataclasses from typing import TypeVar @@ -338,15 +337,12 @@ def __post_init__( if settings.parametric_link_names is not None else 0.0 ) - self.parametric_link_densities = ( - copy.deepcopy(settings.initial_densities) - if settings.initial_densities is not None - else 0.0 - ) if isinstance( kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric ): + self.parametric_link_densities = kin_dyn_object.get_original_densities() + print("parametric_link_densities", self.parametric_link_densities) total_mass_fun = kin_dyn_object.get_total_mass() self.mass = float( total_mass_fun( @@ -356,6 +352,7 @@ def __post_init__( ) else: self.mass = kin_dyn_object.get_total_mass() + self.parametric_link_densities = 0.0 self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier self.dcc_gain = settings.dcc_gain From 9be912ac40f72c101345839f96e0b008c676a28f Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 11 Mar 2024 11:56:16 +0100 Subject: [PATCH 14/16] Removed leftover print --- src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index 7c692d72..d1aaa5ba 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -342,7 +342,6 @@ def __post_init__( kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric ): self.parametric_link_densities = kin_dyn_object.get_original_densities() - print("parametric_link_densities", self.parametric_link_densities) total_mass_fun = kin_dyn_object.get_total_mass() self.mass = float( total_mass_fun( From bd9aa3a0e4801c3842c5433fb0ca4c08d1c0c785 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 11 Mar 2024 11:56:37 +0100 Subject: [PATCH 15/16] Added the possibility of using parametric models in HumanoidPoseFinder --- .../humanoid_pose_finder/planner.py | 146 ++++++++++++++---- 1 file changed, 119 insertions(+), 27 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 995e5f56..eeb04f76 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -4,8 +4,13 @@ import adam.casadi import adam.model +import adam.numpy +import adam.parametric.casadi import casadi as cs import numpy as np +from adam.parametric.model.parametric_factories.parametric_model import ( + URDFParametricModelFactory, +) import hippopt as hp import hippopt.robot_planning as hp_rp @@ -15,6 +20,7 @@ class Settings: robot_urdf: str = dataclasses.field(default=None) joints_name_list: list[str] = dataclasses.field(default=None) + parametric_link_names: list[str] = dataclasses.field(default=None) contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) @@ -152,6 +158,10 @@ class Variables(hp.OptimizationObject): cls=hp.Variable, factory=hp_rp.HumanoidState ) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + parametric_link_length_multipliers: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + parametric_link_densities: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) references: References = hp.default_composite_field( cls=hp.Parameter, factory=References @@ -172,8 +182,11 @@ class Variables(hp.OptimizationObject): def __post_init__( self, settings: Settings, - kin_dyn_object: adam.casadi.KinDynComputations, - ): + kin_dyn_object: ( + adam.casadi.KinDynComputations + | adam.parametric.casadi.KinDynComputationsParametric + ), + ) -> None: self.state = hp_rp.HumanoidState( contact_point_descriptors=settings.contact_points, number_of_joints=len(settings.joints_name_list), @@ -182,7 +195,26 @@ def __post_init__( contact_point_descriptors=settings.contact_points, number_of_joints=len(settings.joints_name_list), ) - self.mass = kin_dyn_object.get_total_mass() + self.parametric_link_length_multipliers = ( + np.ones(len(settings.parametric_link_names)) + if settings.parametric_link_names is not None + else 0.0 + ) + + if isinstance( + kin_dyn_object, adam.parametric.casadi.KinDynComputationsParametric + ): + self.parametric_link_densities = kin_dyn_object.get_original_densities() + total_mass_fun = kin_dyn_object.get_total_mass() + self.mass = float( + total_mass_fun( + self.parametric_link_length_multipliers, + self.parametric_link_densities, + ).full() + ) + else: + self.mass = kin_dyn_object.get_total_mass() + self.parametric_link_densities = 0.0 self.gravity = settings.gravity self.static_friction = settings.static_friction self.relaxed_complementarity_epsilon = settings.relaxed_complementarity_epsilon @@ -197,13 +229,26 @@ def __init__(self, settings: Settings): self.settings = copy.deepcopy(settings) - self.kin_dyn_object = adam.casadi.KinDynComputations( - urdfstring=self.settings.robot_urdf, - joints_name_list=self.settings.joints_name_list, - root_link=self.settings.root_link, - gravity=self.settings.gravity, - f_opts=self.settings.casadi_function_options, - ) + if self.settings.parametric_link_names is not None: + self.parametric_model = True + self.kin_dyn_object = adam.parametric.casadi.KinDynComputationsParametric( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + links_name_list=self.settings.parametric_link_names, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + else: + self.parametric_model = False + self.kin_dyn_object = adam.casadi.KinDynComputations( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + structure = Variables( settings=self.settings, kin_dyn_object=self.kin_dyn_object ) @@ -307,6 +352,8 @@ def _get_function_inputs_dict(self): "second_point_name": "p_1", "desired_yaw_name": "yd", "desired_height_name": "hd", + "parametric_link_length_multipliers_name": "pi_l", + "parametric_link_densities_name": "pi_d", "options": self.settings.casadi_function_options, } return function_inputs @@ -332,11 +379,20 @@ def _add_kinematics_constraints( com_kinematics_fun = hp_rp.center_of_mass_position_from_kinematics( kindyn_object=self.kin_dyn_object, **function_inputs ) - com_kinematics = com_kinematics_fun( - pb=variables.state.kinematics.base.position, - qb=normalized_quaternion, - s=variables.state.kinematics.joints.positions, - )["com_position"] + if self.parametric_model: + com_kinematics = com_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + pi_l=variables.parametric_link_length_multipliers, + pi_d=variables.parametric_link_densities, + )["com_position"] + else: + com_kinematics = com_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + )["com_position"] problem.add_constraint( expression=cs.MX(variables.state.com == com_kinematics), name="com_kinematics_consistency", @@ -405,12 +461,22 @@ def _add_kinematics_regularization( target_frame=self.settings.desired_frame_quaternion_cost_frame_name, **function_inputs, ) - rotation_error_kinematics = rotation_error_kinematics_fun( - pb=variables.state.kinematics.base.position, - qb=normalized_quaternion, - s=variables.state.kinematics.joints.positions, - qd=variables.references.frame_quaternion_xyzw, - )["rotation_error"] + if self.parametric_model: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + pi_l=variables.parametric_link_length_multipliers, + pi_d=variables.parametric_link_densities, + qd=variables.references.frame_quaternion_xyzw, + )["rotation_error"] + else: + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + qd=variables.references.frame_quaternion_xyzw, + )["rotation_error"] problem.add_cost( expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), name="frame_rotation_error", @@ -465,12 +531,22 @@ def _add_contact_kinematic_consistency( ) # Consistency between the contact position and the kinematics - point_kinematics = point_kinematics_functions[descriptor.foot_frame]( - pb=variables.state.kinematics.base.position, - qb=normalized_quaternion, - s=variables.state.kinematics.joints.positions, - p_parent=descriptor.position_in_foot_frame, - )["point_position"] + if self.parametric_model: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + pi_l=variables.parametric_link_length_multipliers, + pi_d=variables.parametric_link_densities, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] + else: + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] problem.add_constraint( expression=cs.MX(point.p == point_kinematics), name=point.p.name() + "_kinematics_consistency", @@ -575,4 +651,20 @@ def solve(self) -> hp.Output[Variables]: return self.op.problem.solve() def get_adam_model(self) -> adam.model.Model: + if self.parametric_model: + guess = self.get_initial_guess() + original_length = guess.parametric_link_length_multipliers + original_density = guess.parametric_link_densities + factory = URDFParametricModelFactory( + path=self.settings.robot_urdf, + math=adam.numpy.numpy_like.SpatialMath(), + links_name_list=self.settings.parametric_link_names, + length_multiplier=original_length, + densities=original_density, + ) + model = adam.model.Model.build( + factory=factory, joints_name_list=self.settings.joints_name_list + ) + return model + return self.kin_dyn_object.rbdalgos.model From b63348650a6803f05b02a455f7a7abd1b060d1e3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 11 Mar 2024 12:22:08 +0100 Subject: [PATCH 16/16] Using a parametric model also for the guess and initial conditions in the parametric periodic walking --- .../main_periodic_step_parametric.py | 100 ++++++++++++------ 1 file changed, 69 insertions(+), 31 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py index af92bc1b..1a69d74d 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step_parametric.py @@ -48,36 +48,6 @@ def get_planner_settings() -> walking_settings.Settings: "r_ankle_pitch", "r_ankle_roll", ] - settings.parametric_link_names = [ - "r_upper_arm", - "r_forearm", - "l_hip_3", - "l_lower_leg", - "root_link", - "torso_1", - "torso_2", - "chest", - ] - # settings.parametric_link_densities = [ - # 1661.6863265236248, - # 727.4313078156689, - # 600.8642717368293, - # 2134.3111071426842, - # 2129.295296396375, - # 1199.0762240824756, - # 893.1076351798705, - # 626.6027187152905, - # ] - # settings.parametric_link_length_multipliers = [ - # 1.0, - # 1.0, - # 1.0, - # 1.0, - # 1.0, - # 1.0, - # 1.0, - # 1.0, - # ] number_of_joints = len(settings.joints_name_list) idyntree_model_loader = idyntree.ModelLoader() @@ -176,6 +146,7 @@ def get_pose_finder_settings( settings = pose_finder.Settings() settings.robot_urdf = input_settings.robot_urdf settings.joints_name_list = input_settings.joints_name_list + settings.parametric_link_names = input_settings.parametric_link_names settings.root_link = input_settings.root_link settings.desired_frame_quaternion_cost_frame_name = ( @@ -228,6 +199,8 @@ def compute_state( desired_com_position: np.ndarray, desired_left_foot_pose: liecasadi.SE3, desired_right_foot_pose: liecasadi.SE3, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, ) -> hp_rp.HumanoidState: desired_joints = np.deg2rad( [ @@ -285,7 +258,15 @@ def compute_state( pf_ref.state.kinematics.joints.positions = desired_joints - pf_input.set_references(pf_ref) + pf_guess = pf_input.get_initial_guess() + pf_guess.references = pf_ref + if pf_parametric_link_densities is not None: + pf_guess.parametric_link_densities = pf_parametric_link_densities + if pf_parametric_link_length_multipliers is not None: + pf_guess.parametric_link_length_multipliers = ( + pf_parametric_link_length_multipliers + ) + pf_input.set_initial_guess(pf_guess) output_pf = pf_input.solve() return output_pf.values.state @@ -295,6 +276,8 @@ def compute_initial_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, contact_guess: hp_rp.FeetContactPhasesDescriptor, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, ) -> walking_variables.ExtendedHumanoidState: desired_left_foot_pose = contact_guess.left[0].transform desired_right_foot_pose = contact_guess.right[0].transform @@ -308,6 +291,8 @@ def compute_initial_state( desired_com_position=desired_com_position, desired_left_foot_pose=desired_left_foot_pose, desired_right_foot_pose=desired_right_foot_pose, + pf_parametric_link_densities=pf_parametric_link_densities, + pf_parametric_link_length_multipliers=pf_parametric_link_length_multipliers, ) output_state = walking_variables.ExtendedHumanoidState() @@ -324,6 +309,8 @@ def compute_middle_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, contact_guess: hp_rp.FeetContactPhasesDescriptor, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, ) -> hp_rp.HumanoidState: desired_left_foot_pose = contact_guess.left[1].transform desired_right_foot_pose = contact_guess.right[0].transform @@ -337,6 +324,8 @@ def compute_middle_state( desired_com_position=desired_com_position, desired_left_foot_pose=desired_left_foot_pose, desired_right_foot_pose=desired_right_foot_pose, + pf_parametric_link_densities=pf_parametric_link_densities, + pf_parametric_link_length_multipliers=pf_parametric_link_length_multipliers, ) @@ -344,6 +333,8 @@ def compute_final_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, contact_guess: hp_rp.FeetContactPhasesDescriptor, + pf_parametric_link_densities: list[float] | None, + pf_parametric_link_length_multipliers: list[float] | None, ) -> hp_rp.HumanoidState: desired_left_foot_pose = contact_guess.left[1].transform desired_right_foot_pose = contact_guess.right[1].transform @@ -357,6 +348,8 @@ def compute_final_state( desired_com_position=desired_com_position, desired_left_foot_pose=desired_left_foot_pose, desired_right_foot_pose=desired_right_foot_pose, + pf_parametric_link_densities=pf_parametric_link_densities, + pf_parametric_link_length_multipliers=pf_parametric_link_length_multipliers, ) @@ -388,6 +381,39 @@ def get_references( logging.basicConfig(level=logging.INFO) planner_settings = get_planner_settings() + + planner_settings.parametric_link_names = [ + "r_upper_arm", + "r_forearm", + "l_hip_3", + "l_lower_leg", + "root_link", + "torso_1", + "torso_2", + "chest", + ] + + parametric_link_densities = [ + 1578.8230690646876, + 687.9855671524874, + 568.2817642184916, + 1907.2410446310623, + 2013.8319822728106, + 1134.0550335996697, + 844.6779189491116, + 628.0724496264946, + ] + parametric_link_length_multipliers = [ + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 1.0, + 2.0, + ] + planner = walking_planner.Planner(settings=planner_settings) pf_settings = get_pose_finder_settings(input_settings=planner_settings) @@ -448,12 +474,16 @@ def get_references( input_settings=planner_settings, pf_input=pf, contact_guess=contact_phases_guess, + pf_parametric_link_densities=parametric_link_densities, + pf_parametric_link_length_multipliers=parametric_link_length_multipliers, ) final_state = compute_final_state( input_settings=planner_settings, pf_input=pf, contact_guess=contact_phases_guess, + pf_parametric_link_densities=parametric_link_densities, + pf_parametric_link_length_multipliers=parametric_link_length_multipliers, ) final_state.centroidal_momentum = np.zeros((6, 1)) @@ -461,6 +491,8 @@ def get_references( input_settings=planner_settings, pf_input=pf, contact_guess=contact_phases_guess, + pf_parametric_link_densities=parametric_link_densities, + pf_parametric_link_length_multipliers=parametric_link_length_multipliers, ) first_half_guess_length = planner_settings.horizon_length // 2 @@ -500,6 +532,12 @@ def get_references( ] planner_guess.initial_state = initial_state planner_guess.final_state = final_state + if parametric_link_densities is not None: + planner_guess.parametric_link_densities = parametric_link_densities + if parametric_link_length_multipliers is not None: + planner_guess.parametric_link_length_multipliers = ( + parametric_link_length_multipliers + ) planner.set_initial_guess(planner_guess) output = planner.solve()