From 9ad4e1541a4fead51cc4b467ce315d0742fd5195 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 2 Aug 2023 18:09:26 +0200 Subject: [PATCH 001/100] Initial drafting of humanoid walking on flat ground --- .../humanoid_walking_flat_ground.py | 110 ++++++++++++++++++ 1 file changed, 110 insertions(+) create mode 100644 src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py new file mode 100644 index 00000000..e232d513 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py @@ -0,0 +1,110 @@ +import dataclasses + +import adam.casadi +import numpy as np + +import hippopt as hp +import hippopt.robot_planning as hp_rp + + +@dataclasses.dataclass +class FeetContactPoints(hp.OptimizationObject): + left: list[hp_rp.ContactPoint] = hp.default_storage_field(hp.Variable) + right: list[hp_rp.ContactPoint] = hp.default_storage_field(hp.Variable) + + +@dataclasses.dataclass +class FeetContactPointDescriptors: + left: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) + right: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + contact_points: FeetContactPoints = dataclasses.field( + default_factory=FeetContactPoints + ) + com: hp.StorageType = hp.default_storage_field(hp.Variable) + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + base_position: hp.StorageType = hp.default_storage_field(hp.Variable) + base_linear_velocity: hp.StorageType = hp.default_storage_field(hp.Variable) + base_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Variable) + base_quaternion_velocity_xyzw: hp.StorageType = hp.default_storage_field( + hp.Variable + ) + joint_positions: hp.StorageType = hp.default_storage_field(hp.Variable) + joint_velocities: hp.StorageType = hp.default_storage_field(hp.Variable) + + dt: hp.StorageType = hp.default_storage_field(hp.Parameter) + gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + contact_point_descriptors: dataclasses.InitVar[ + FeetContactPointDescriptors + ] = dataclasses.field(default=None) + + def __post_init__( + self, + number_of_joints: int, + contact_point_descriptors: FeetContactPointDescriptors, + ): + self.contact_points.left = [ + hp_rp.ContactPoint(descriptor=point) + for point in contact_point_descriptors.left + ] + self.contact_points.right = [ + hp_rp.ContactPoint(descriptor=point) + for point in contact_point_descriptors.right + ] + + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(3) + self.base_position = np.zeros(3) + self.base_linear_velocity = np.zeros(3) + self.base_quaternion_xyzw = np.zeros(4) + self.base_quaternion_velocity_xyzw = np.zeros(4) + self.joint_positions = np.zeros(number_of_joints) + self.joint_velocities = np.zeros(number_of_joints) + self.dt = 0.1 * np.ones(1) + self.gravity = np.zeros(3) + self.gravity[2] = -9.81 + + +@dataclasses.dataclass +class Settings: + robot_urdf: str = dataclasses.field(default=None) + joints_name_list: list[str] = dataclasses.field(default=None) + contact_points: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default=None) + root_link: str = dataclasses.field(default=None) + gravity: np.array = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) + + def __post_init__(self): + self.casadi_function_options = ( + self.casadi_function_options + if isinstance(self.casadi_function_options, dict) + else {} + ) + self.root_link = "root_link" + self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) + + def is_valid(self): + return ( + self.robot_urdf is not None + and self.joints_name_list is not None + and self.contact_points is not None + ) + + +class HumanoidWalkingFlatGround: + def __init__(self, settings: Settings) -> None: + if not settings.is_valid(): + raise ValueError("Settings are not valid") + self.settings = 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, + ) From cb6466f12b5d53b716f46d5e38627830b66d32d9 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 16:18:33 +0200 Subject: [PATCH 002/100] Moved humanoid_walking_flat_ground planner to dedicated folder --- .../humanoid_walking_flat_ground.py | 110 ------------ .../humanoid_walking_flat_ground/main.py | 4 + .../humanoid_walking_flat_ground/planner.py | 167 ++++++++++++++++++ 3 files changed, 171 insertions(+), 110 deletions(-) delete mode 100644 src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py create mode 100644 src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py create mode 100644 src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py deleted file mode 100644 index e232d513..00000000 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground.py +++ /dev/null @@ -1,110 +0,0 @@ -import dataclasses - -import adam.casadi -import numpy as np - -import hippopt as hp -import hippopt.robot_planning as hp_rp - - -@dataclasses.dataclass -class FeetContactPoints(hp.OptimizationObject): - left: list[hp_rp.ContactPoint] = hp.default_storage_field(hp.Variable) - right: list[hp_rp.ContactPoint] = hp.default_storage_field(hp.Variable) - - -@dataclasses.dataclass -class FeetContactPointDescriptors: - left: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) - right: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) - - -@dataclasses.dataclass -class Variables(hp.OptimizationObject): - contact_points: FeetContactPoints = dataclasses.field( - default_factory=FeetContactPoints - ) - com: hp.StorageType = hp.default_storage_field(hp.Variable) - centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) - base_position: hp.StorageType = hp.default_storage_field(hp.Variable) - base_linear_velocity: hp.StorageType = hp.default_storage_field(hp.Variable) - base_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Variable) - base_quaternion_velocity_xyzw: hp.StorageType = hp.default_storage_field( - hp.Variable - ) - joint_positions: hp.StorageType = hp.default_storage_field(hp.Variable) - joint_velocities: hp.StorageType = hp.default_storage_field(hp.Variable) - - dt: hp.StorageType = hp.default_storage_field(hp.Parameter) - gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) - - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) - contact_point_descriptors: dataclasses.InitVar[ - FeetContactPointDescriptors - ] = dataclasses.field(default=None) - - def __post_init__( - self, - number_of_joints: int, - contact_point_descriptors: FeetContactPointDescriptors, - ): - self.contact_points.left = [ - hp_rp.ContactPoint(descriptor=point) - for point in contact_point_descriptors.left - ] - self.contact_points.right = [ - hp_rp.ContactPoint(descriptor=point) - for point in contact_point_descriptors.right - ] - - self.com = np.zeros(3) - self.centroidal_momentum = np.zeros(3) - self.base_position = np.zeros(3) - self.base_linear_velocity = np.zeros(3) - self.base_quaternion_xyzw = np.zeros(4) - self.base_quaternion_velocity_xyzw = np.zeros(4) - self.joint_positions = np.zeros(number_of_joints) - self.joint_velocities = np.zeros(number_of_joints) - self.dt = 0.1 * np.ones(1) - self.gravity = np.zeros(3) - self.gravity[2] = -9.81 - - -@dataclasses.dataclass -class Settings: - robot_urdf: str = dataclasses.field(default=None) - joints_name_list: list[str] = dataclasses.field(default=None) - contact_points: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default=None) - root_link: str = dataclasses.field(default=None) - gravity: np.array = dataclasses.field(default=None) - casadi_function_options: dict = dataclasses.field(default_factory=dict) - - def __post_init__(self): - self.casadi_function_options = ( - self.casadi_function_options - if isinstance(self.casadi_function_options, dict) - else {} - ) - self.root_link = "root_link" - self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) - - def is_valid(self): - return ( - self.robot_urdf is not None - and self.joints_name_list is not None - and self.contact_points is not None - ) - - -class HumanoidWalkingFlatGround: - def __init__(self, settings: Settings) -> None: - if not settings.is_valid(): - raise ValueError("Settings are not valid") - self.settings = 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, - ) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py new file mode 100644 index 00000000..d59a411d --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -0,0 +1,4 @@ +import hippopt.turnkey_planners.humanoid_walking_flat_ground.planner as walking_planner + +if __name__ == "__main__": + planner_settings = walking_planner.Settings() diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py new file mode 100644 index 00000000..6a4a5b3d --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -0,0 +1,167 @@ +import dataclasses +import typing + +import adam.casadi +import numpy as np + +import hippopt as hp +import hippopt.integrators as hp_int +import hippopt.robot_planning as hp_rp + + +@dataclasses.dataclass +class ExtendedContactPoint(hp_rp.ContactPoint): + u_v: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: + super().__post_init__(input_descriptor) + self.u_v = np.zeros(3) + + +@dataclasses.dataclass +class FeetContactPoints(hp.OptimizationObject): + left: list[ExtendedContactPoint] = hp.default_composite_field() + right: list[ExtendedContactPoint] = hp.default_composite_field() + + +@dataclasses.dataclass +class FeetContactPointDescriptors: + left: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) + right: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) + + +@dataclasses.dataclass +class Settings: + robot_urdf: str = dataclasses.field(default=None) + joints_name_list: list[str] = dataclasses.field(default=None) + contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) + root_link: str = dataclasses.field(default=None) + gravity: np.array = dataclasses.field(default=None) + horizon_length: int = dataclasses.field(default=None) + integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) + + def __post_init__(self) -> None: + self.casadi_function_options = ( + self.casadi_function_options + if isinstance(self.casadi_function_options, dict) + else {} + ) + 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 + + def is_valid(self) -> bool: + 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 + ) + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + contact_points: FeetContactPoints | list[ + FeetContactPoints + ] = hp.default_composite_field() + com: hp.StorageType = hp.default_storage_field(hp.Variable) + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + kinematics: hp_rp.FloatingBaseSystem = hp.default_composite_field() + + com_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) + + dt: hp.StorageType = hp.default_storage_field(hp.Parameter) + gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) + + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) + kin_dyn_object: dataclasses.InitVar[ + adam.casadi.KinDynComputations + ] = dataclasses.field(default=None) + + def __post_init__( + self, + settings: Settings, + kin_dyn_object: adam.casadi.KinDynComputations, + ) -> None: + self.contact_points.left = [ + hp_rp.ContactPoint(descriptor=point) + for point in settings.contact_points.left + ] + self.contact_points.right = [ + hp_rp.ContactPoint(descriptor=point) + for point in settings.contact_points.right + ] + + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(6) + self.kinematics = hp_rp.FloatingBaseSystem(kin_dyn_object.NDoF) + self.dt = 0.1 + self.gravity = kin_dyn_object.g[:, 3] + self.mass = kin_dyn_object.get_total_mass() + + +class HumanoidWalkingFlatGround: + def __init__(self, settings: Settings) -> None: + if not settings.is_valid(): + raise ValueError("Settings are not valid") + self.settings = 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.variables = Variables( + settings=self.settings, kin_dyn_object=self.kin_dyn_object + ) + + self.ocp = hp.OptimalControlProblem.create( + self.variables, horizon_length=self.settings.horizon_length + ) + + problem = self.ocp.problem + sym = self.ocp.symbolic_structure + + default_integrator = self.settings.integrator + + for point in sym.contact_points.left + sym.contact_points.right: + problem.add_dynamics( + hp.dot(point.force) == point.f_dot, + x0=point.f0, + integrator=default_integrator, + ) + problem.add_dynamics( + hp.dot(point.p) == point.v, x0=point.p0, integrator=default_integrator + ) + + problem.add_dynamics( + hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, + x0=sym.kinematics.base.initial_position, + integrator=default_integrator, + ) + + problem.add_dynamics( + hp.dot(sym.kinematics.base.quaternion_xyzw) + == sym.kinematics.base.quaternion_velocity_xyzw, + x0=sym.kinematics.base.initial_quaternion_xyzw, + integrator=default_integrator, + ) + + problem.add_dynamics( + hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, + x0=sym.kinematics.joints.initial_positions, + integrator=default_integrator, + ) + + problem.add_dynamics( + hp.dot(sym.com) == sym.centroidal_momentum[:3] / sym.mass, + x0=sym.com_initial, + integrator=default_integrator, + ) + + def set_initial_conditions(self) -> None: # TODO: fill + pass From dce6b51c452b8bb666f9d698b5723c9f2c44c0b8 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 17:46:58 +0200 Subject: [PATCH 003/100] Added all dynamics in flat ground planner --- .../humanoid_walking_flat_ground/planner.py | 31 +++++++++++++++++-- 1 file changed, 29 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 6a4a5b3d..06f11ca0 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -71,6 +71,7 @@ class Variables(hp.OptimizationObject): kinematics: hp_rp.FloatingBaseSystem = hp.default_composite_field() com_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) + centroidal_momentum_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) dt: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -101,6 +102,9 @@ def __post_init__( self.gravity = kin_dyn_object.g[:, 3] self.mass = kin_dyn_object.get_total_mass() + self.com_initial = np.zeros(3) + self.centroidal_momentum_initial = np.zeros(6) + class HumanoidWalkingFlatGround: def __init__(self, settings: Settings) -> None: @@ -128,15 +132,27 @@ def __init__(self, settings: Settings) -> None: default_integrator = self.settings.integrator + function_inputs = { + "mass_name": sym.mass.name(), + "momentum_name": sym.centroidal_momentum.name(), + "com_name": sym.com.name(), + "gravity_name": sym.gravity.name(), + "point_position_names": [], + "point_force_names": [], + "options": self.settings.casadi_function_options, + } + for point in sym.contact_points.left + sym.contact_points.right: problem.add_dynamics( - hp.dot(point.force) == point.f_dot, + hp.dot(point.f) == point.f_dot, x0=point.f0, integrator=default_integrator, ) problem.add_dynamics( hp.dot(point.p) == point.v, x0=point.p0, integrator=default_integrator ) + function_inputs["point_position_names"].append(point.p.name()) + function_inputs["point_force_names"].append(point.f.name()) problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, @@ -157,11 +173,22 @@ def __init__(self, settings: Settings) -> None: integrator=default_integrator, ) + com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) + centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( + **function_inputs + ) + problem.add_dynamics( - hp.dot(sym.com) == sym.centroidal_momentum[:3] / sym.mass, + hp.dot(sym.com) == com_dynamics, x0=sym.com_initial, integrator=default_integrator, ) + problem.add_dynamics( + hp.dot(sym.centroidal_momentum) == centroidal_dynamics, + x0=sym.centroidal_momentum_initial, + integrator=default_integrator, + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 119945ac686a7b690a2824d4642f2cd10ffb488f Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 18:24:57 +0200 Subject: [PATCH 004/100] Added planar complementarity to flat ground planner --- .../humanoid_walking_flat_ground/planner.py | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 06f11ca0..3b98be8b 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -2,6 +2,7 @@ import typing import adam.casadi +import casadi as cs import numpy as np import hippopt as hp @@ -39,6 +40,8 @@ class Settings: gravity: np.array = dataclasses.field(default=None) horizon_length: int = dataclasses.field(default=None) integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) + terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) + planar_dcc_height_multiplier: float = dataclasses.field(default=None) casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -50,6 +53,8 @@ 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 def is_valid(self) -> bool: return ( @@ -75,6 +80,9 @@ class Variables(hp.OptimizationObject): dt: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) + planar_dcc_height_multiplier: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -105,6 +113,8 @@ def __post_init__( self.com_initial = np.zeros(3) self.centroidal_momentum_initial = np.zeros(6) + self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier + class HumanoidWalkingFlatGround: def __init__(self, settings: Settings) -> None: @@ -142,6 +152,12 @@ def __init__(self, settings: Settings) -> None: "options": self.settings.casadi_function_options, } + dcc_planar_fun = hp_rp.dcc_planar_complementarity( + terrain=self.settings.terrain, + point_position_name="p", + **function_inputs, + ) + for point in sym.contact_points.left + sym.contact_points.right: problem.add_dynamics( hp.dot(point.f) == point.f_dot, @@ -151,6 +167,15 @@ def __init__(self, settings: Settings) -> None: problem.add_dynamics( hp.dot(point.p) == point.v, x0=point.p0, integrator=default_integrator ) + + dcc_planar = dcc_planar_fun( + p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v + )["planar_complementarity"] + + problem.add_expression_to_horizon( + expression=cs.MX(point.v == dcc_planar), apply_to_first_elements=True + ) + function_inputs["point_position_names"].append(point.p.name()) function_inputs["point_force_names"].append(point.f.name()) From 5a76166dee25eb00b930ce8458c821a5a47dda00 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 11:39:44 +0200 Subject: [PATCH 005/100] Added dcc complementarity to flat ground planner Renamed a parameter in dcc_complementarity_margin for better clarity --- .../humanoid_walking_flat_ground/planner.py | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 3b98be8b..39aa562a 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -149,12 +149,24 @@ def __init__(self, settings: Settings) -> None: "gravity_name": sym.gravity.name(), "point_position_names": [], "point_force_names": [], + "point_position_name": "p", + "point_force_name": "f", + "point_velocity_name": "v", + "point_force_derivative_name": "f_dot", + "point_position_control_name": "u_p", + "height_multiplier_name": "kt", + "dcc_gain_name": "k_bs", + "dcc_epsilon_name": "eps", "options": self.settings.casadi_function_options, } dcc_planar_fun = hp_rp.dcc_planar_complementarity( terrain=self.settings.terrain, - point_position_name="p", + **function_inputs, + ) + + dcc_margin_fun = hp_rp.dcc_complementarity_margin( + terrain=self.settings.terrain, **function_inputs, ) @@ -176,6 +188,19 @@ def __init__(self, settings: Settings) -> None: expression=cs.MX(point.v == dcc_planar), apply_to_first_elements=True ) + dcc_margin = dcc_margin_fun( + p=point.p, + f=point.f, + v=point.v, + f_dot=point.f_dot, + k_bs=sym.settings.dcc_gain, + eps=sym.settings.dcc_epsilon, + )["dcc_complementarity_margin"] + + problem.add_expression_to_horizon( + expression=cs.MX(dcc_margin >= 0), apply_to_first_elements=True + ) + function_inputs["point_position_names"].append(point.p.name()) function_inputs["point_force_names"].append(point.f.name()) From ce4dd26de2d1f5f1e3513c67d7e318a58b8e92cd Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 12:32:39 +0200 Subject: [PATCH 006/100] Added friction expression to planner and fixed the use of dcc expressions --- .../humanoid_walking_flat_ground/planner.py | 52 +++++++++++++++++-- 1 file changed, 47 insertions(+), 5 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 39aa562a..b216def7 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -1,3 +1,4 @@ +import copy import dataclasses import typing @@ -42,6 +43,9 @@ class Settings: integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) planar_dcc_height_multiplier: float = dataclasses.field(default=None) + dcc_gain: float = dataclasses.field(default=None) + dcc_epsilon: float = dataclasses.field(default=None) + static_friction: float = dataclasses.field(default=None) casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -55,6 +59,9 @@ def __post_init__(self) -> None: 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 def is_valid(self) -> bool: return ( @@ -83,6 +90,9 @@ class Variables(hp.OptimizationObject): planar_dcc_height_multiplier: hp.StorageType = hp.default_storage_field( hp.Parameter ) + dcc_gain: hp.StorageType = hp.default_storage_field(hp.Parameter) + dcc_epsilon: hp.StorageType = hp.default_storage_field(hp.Parameter) + static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -114,13 +124,16 @@ def __post_init__( self.centroidal_momentum_initial = np.zeros(6) self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier + self.dcc_gain = settings.dcc_gain + self.dcc_epsilon = settings.dcc_epsilon + self.static_friction = settings.static_friction class HumanoidWalkingFlatGround: def __init__(self, settings: Settings) -> None: if not settings.is_valid(): raise ValueError("Settings are not valid") - 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, @@ -157,9 +170,12 @@ def __init__(self, settings: Settings) -> None: "height_multiplier_name": "kt", "dcc_gain_name": "k_bs", "dcc_epsilon_name": "eps", + "static_friction_name": "mu_s", "options": self.settings.casadi_function_options, } + self.settings.terrain.change_options(**function_inputs) + dcc_planar_fun = hp_rp.dcc_planar_complementarity( terrain=self.settings.terrain, **function_inputs, @@ -170,6 +186,15 @@ def __init__(self, settings: Settings) -> None: **function_inputs, ) + friction_margin_fun = hp_rp.friction_cone_square_margin( + terrain=self.settings.terrain, **function_inputs + ) + + height_fun = self.settings.terrain.height_function() + normal_force_fun = hp_rp.normal_force_component( + terrain=self.settings.terrain, **function_inputs + ) + for point in sym.contact_points.left + sym.contact_points.right: problem.add_dynamics( hp.dot(point.f) == point.f_dot, @@ -183,7 +208,6 @@ def __init__(self, settings: Settings) -> None: dcc_planar = dcc_planar_fun( p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v )["planar_complementarity"] - problem.add_expression_to_horizon( expression=cs.MX(point.v == dcc_planar), apply_to_first_elements=True ) @@ -193,14 +217,32 @@ def __init__(self, settings: Settings) -> None: f=point.f, v=point.v, f_dot=point.f_dot, - k_bs=sym.settings.dcc_gain, - eps=sym.settings.dcc_epsilon, + k_bs=sym.dcc_gain, + eps=sym.dcc_epsilon, )["dcc_complementarity_margin"] - problem.add_expression_to_horizon( expression=cs.MX(dcc_margin >= 0), apply_to_first_elements=True ) + point_height = height_fun(p=point.p)["point_height"] + problem.add_expression_to_horizon( + expression=cs.MX(point_height >= 0), apply_to_first_elements=False + ) + + normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] + problem.add_expression_to_horizon( + expression=cs.MX(normal_force >= 0), apply_to_first_elements=False + ) + + friction_margin = friction_margin_fun( + p=point.p, + f=point.f, + mu_s=sym.static_friction, + )["friction_cone_square_margin"] + problem.add_expression_to_horizon( + expression=cs.MX(friction_margin >= 0), apply_to_first_elements=False + ) + function_inputs["point_position_names"].append(point.p.name()) function_inputs["point_force_names"].append(point.f.name()) From daabe041bd8c3a6e2f0834dfa129ae3581b065e4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 14:31:19 +0200 Subject: [PATCH 007/100] Added bounds on contact point control inputs --- .../humanoid_walking_flat_ground/planner.py | 26 +++++++++++++++++++ 1 file changed, 26 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index b216def7..489023c7 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -46,6 +46,8 @@ class Settings: dcc_gain: float = dataclasses.field(default=None) dcc_epsilon: float = dataclasses.field(default=None) static_friction: float = dataclasses.field(default=None) + maximum_velocity_control: np.ndarray = dataclasses.field(default=None) + maximum_force_derivative: np.ndarray = dataclasses.field(default=None) casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -62,6 +64,8 @@ def __post_init__(self) -> None: self.dcc_gain = 20.0 self.dcc_epsilon = 0.05 self.static_friction = 0.3 + self.maximum_velocity_control = np.ndarray([2.0, 2.0, 5.0]) + self.maximum_force_derivative = np.ndarray([100.0, 100.0, 100.0]) def is_valid(self) -> bool: return ( @@ -93,6 +97,8 @@ class Variables(hp.OptimizationObject): dcc_gain: hp.StorageType = hp.default_storage_field(hp.Parameter) dcc_epsilon: hp.StorageType = hp.default_storage_field(hp.Parameter) static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_velocity_control: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_force_derivative: hp.StorageType = hp.default_storage_field(hp.Parameter) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -127,6 +133,8 @@ def __post_init__( self.dcc_gain = settings.dcc_gain self.dcc_epsilon = settings.dcc_epsilon self.static_friction = settings.static_friction + self.maximum_velocity_control = settings.maximum_velocity_control + self.maximum_force_derivative = settings.maximum_force_derivative class HumanoidWalkingFlatGround: @@ -243,6 +251,24 @@ def __init__(self, settings: Settings) -> None: expression=cs.MX(friction_margin >= 0), apply_to_first_elements=False ) + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_velocity_control, + point.u_v, + sym.maximum_velocity_control, + ), + apply_to_first_elements=True, + ) + + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_force_control, + point.u_f, + sym.maximum_force_control, + ), + apply_to_first_elements=True, + ) + function_inputs["point_position_names"].append(point.p.name()) function_inputs["point_force_names"].append(point.f.name()) From 269e8915b30e3dded25e63c0515c4863ab45a117 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 15:50:40 +0200 Subject: [PATCH 008/100] Added constraints related to kinematics --- .../humanoid_walking_flat_ground/planner.py | 105 ++++++++++++++++-- 1 file changed, 98 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 489023c7..c3d6b9a6 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -167,9 +167,17 @@ def __init__(self, settings: Settings) -> None: "mass_name": sym.mass.name(), "momentum_name": sym.centroidal_momentum.name(), "com_name": sym.com.name(), + "quaternion_xyzw_name": "q", "gravity_name": sym.gravity.name(), "point_position_names": [], "point_force_names": [], + "point_position_in_frame_name": "p_parent", + "base_position_name": "pb", + "base_quaternion_xyzw_name": "qb", + "joint_positions_name": "s", + "base_position_derivative_name": "pb_dot", + "base_quaternion_xyzw_derivative_name": "qb_dot", + "joint_velocities_name": "s_dot", "point_position_name": "p", "point_force_name": "f", "point_velocity_name": "v", @@ -182,37 +190,50 @@ def __init__(self, settings: Settings) -> None: "options": self.settings.casadi_function_options, } + # Normalized quaternion computation + normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( + **function_inputs + ) + normalized_quaternion = normalized_quaternion_fun( + q=sym.kinematics.base.quaternion_xyzw + )["quaternion_normalized"] + + # Align names used in the terrain function with those in function_inputs self.settings.terrain.change_options(**function_inputs) + # Definition of contact constraint functions dcc_planar_fun = hp_rp.dcc_planar_complementarity( terrain=self.settings.terrain, **function_inputs, ) - dcc_margin_fun = hp_rp.dcc_complementarity_margin( terrain=self.settings.terrain, **function_inputs, ) - friction_margin_fun = hp_rp.friction_cone_square_margin( terrain=self.settings.terrain, **function_inputs ) - height_fun = self.settings.terrain.height_function() normal_force_fun = hp_rp.normal_force_component( terrain=self.settings.terrain, **function_inputs ) + point_kinematics_functions = {} + for point in sym.contact_points.left + sym.contact_points.right: + # dot(f) = f_dot problem.add_dynamics( hp.dot(point.f) == point.f_dot, x0=point.f0, integrator=default_integrator, ) + + # dot(p) = v problem.add_dynamics( hp.dot(point.p) == point.v, x0=point.p0, integrator=default_integrator ) + # Planar complementarity dcc_planar = dcc_planar_fun( p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v )["planar_complementarity"] @@ -220,6 +241,7 @@ def __init__(self, settings: Settings) -> None: expression=cs.MX(point.v == dcc_planar), apply_to_first_elements=True ) + # Normal complementarity dcc_margin = dcc_margin_fun( p=point.p, f=point.f, @@ -232,16 +254,19 @@ def __init__(self, settings: Settings) -> None: expression=cs.MX(dcc_margin >= 0), apply_to_first_elements=True ) + # Point height greater than zero point_height = height_fun(p=point.p)["point_height"] problem.add_expression_to_horizon( expression=cs.MX(point_height >= 0), apply_to_first_elements=False ) + # Normal force greater than zero normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] problem.add_expression_to_horizon( expression=cs.MX(normal_force >= 0), apply_to_first_elements=False ) + # Friction friction_margin = friction_margin_fun( p=point.p, f=point.f, @@ -251,6 +276,7 @@ def __init__(self, settings: Settings) -> None: expression=cs.MX(friction_margin >= 0), apply_to_first_elements=False ) + # Bounds on contact velocity control inputs problem.add_expression_to_horizon( expression=cs.Opti_bounded( -sym.maximum_velocity_control, @@ -260,6 +286,7 @@ def __init__(self, settings: Settings) -> None: apply_to_first_elements=True, ) + # Bounds on contact force control inputs problem.add_expression_to_horizon( expression=cs.Opti_bounded( -sym.maximum_force_control, @@ -269,15 +296,41 @@ def __init__(self, settings: Settings) -> None: apply_to_first_elements=True, ) + # Creation of contact kinematics consistency functions + descriptor = point.descriptor + if descriptor.foot_frame not in point_kinematics_functions: + point_kinematics_functions[ + descriptor.foot_frame + ] = hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) + + # Consistency between the contact position and the kinematics + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=sym.kinematics.base.position, + qb=normalized_quaternion, + s=sym.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, + ) + function_inputs["point_position_names"].append(point.p.name()) function_inputs["point_force_names"].append(point.f.name()) + # dot(pb) = pb_dot (base position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, x0=sym.kinematics.base.initial_position, integrator=default_integrator, ) + # dot(q) = q_dot (base quaternion dynamics) problem.add_dynamics( hp.dot(sym.kinematics.base.quaternion_xyzw) == sym.kinematics.base.quaternion_velocity_xyzw, @@ -285,28 +338,66 @@ def __init__(self, settings: Settings) -> None: integrator=default_integrator, ) + # dot(s) = s_dot (joint position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, x0=sym.kinematics.joints.initial_positions, integrator=default_integrator, ) + # dot(com) = h_g[:3]/m (center of mass dynamics) com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) - centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( - **function_inputs - ) - problem.add_dynamics( hp.dot(sym.com) == com_dynamics, x0=sym.com_initial, integrator=default_integrator, ) + # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) + centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( + **function_inputs + ) problem.add_dynamics( hp.dot(sym.centroidal_momentum) == centroidal_dynamics, x0=sym.centroidal_momentum_initial, integrator=default_integrator, ) + # Unitary quaternion + problem.add_expression_to_horizon( + expression=cs.MX(cs.sumsqr(sym.kinematics.base.quaternion_xyzw) == 1), + apply_to_first_elements=False, + ) + + # Consistency of com position with kinematics + com_kinematics_fun = hp_rp.center_of_mass_position_from_kinematics( + kindyn_object=self.kin_dyn_object, **function_inputs + ) + com_kinematics = com_kinematics_fun( + pb=sym.kinematics.base.position, + qb=normalized_quaternion, + s=sym.kinematics.joints.positions, + )["com_position"] + problem.add_expression_to_horizon( + expression=cs.MX(sym.com == com_kinematics), apply_to_first_elements=False + ) + + # Consistency of centroidal momentum (angular part only) with kinematics + centroidal_kinematics_fun = hp_rp.centroidal_momentum_from_kinematics( + kindyn_object=self.kin_dyn_object, **function_inputs + ) + centroidal_kinematics = centroidal_kinematics_fun( + pb=sym.kinematics.base.position, + qb=normalized_quaternion, + s=sym.kinematics.joints.positions, + pb_dot=sym.kinematics.base.linear_velocity, + qb_dot=sym.kinematics.base.quaternion_velocity_xyzw, + s_dot=sym.kinematics.joints.velocities, + )["h_g"] + problem.add_expression_to_horizon( + expression=cs.MX(sym.centroidal_momentum[3:] == centroidal_kinematics[3:]), + apply_to_first_elements=True, + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From d084d844ba3f2bc37146822c08dbdbd9462cbce1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 18:05:43 +0200 Subject: [PATCH 009/100] Added names to expressions In multiple shooting solver, exploit the base name in case the initial condition name is not set --- .../humanoid_walking_flat_ground/planner.py | 41 +++++++++++++++---- 1 file changed, 34 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index c3d6b9a6..39f62c71 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -48,6 +48,7 @@ class Settings: static_friction: float = dataclasses.field(default=None) maximum_velocity_control: np.ndarray = dataclasses.field(default=None) maximum_force_derivative: np.ndarray = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -226,11 +227,15 @@ def __init__(self, settings: Settings) -> None: hp.dot(point.f) == point.f_dot, x0=point.f0, integrator=default_integrator, + name=point.f.name() + "_dynamics", ) # dot(p) = v problem.add_dynamics( - hp.dot(point.p) == point.v, x0=point.p0, integrator=default_integrator + hp.dot(point.p) == point.v, + x0=point.p0, + integrator=default_integrator, + name=point.p.name() + "_dynamics", ) # Planar complementarity @@ -238,7 +243,9 @@ def __init__(self, settings: Settings) -> None: p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v )["planar_complementarity"] problem.add_expression_to_horizon( - expression=cs.MX(point.v == dcc_planar), apply_to_first_elements=True + expression=cs.MX(point.v == dcc_planar), + apply_to_first_elements=True, + name=point.p.name() + "_planar_complementarity", ) # Normal complementarity @@ -251,19 +258,25 @@ def __init__(self, settings: Settings) -> None: eps=sym.dcc_epsilon, )["dcc_complementarity_margin"] problem.add_expression_to_horizon( - expression=cs.MX(dcc_margin >= 0), apply_to_first_elements=True + expression=cs.MX(dcc_margin >= 0), + apply_to_first_elements=True, + name=point.p.name() + "_dcc", ) # Point height greater than zero point_height = height_fun(p=point.p)["point_height"] problem.add_expression_to_horizon( - expression=cs.MX(point_height >= 0), apply_to_first_elements=False + expression=cs.MX(point_height >= 0), + apply_to_first_elements=False, + name=point.p.name() + "_height", ) # Normal force greater than zero normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] problem.add_expression_to_horizon( - expression=cs.MX(normal_force >= 0), apply_to_first_elements=False + expression=cs.MX(normal_force >= 0), + apply_to_first_elements=False, + name=point.f.name() + "_normal", ) # Friction @@ -273,7 +286,9 @@ def __init__(self, settings: Settings) -> None: mu_s=sym.static_friction, )["friction_cone_square_margin"] problem.add_expression_to_horizon( - expression=cs.MX(friction_margin >= 0), apply_to_first_elements=False + expression=cs.MX(friction_margin >= 0), + apply_to_first_elements=False, + name=point.f.name() + "_friction", ) # Bounds on contact velocity control inputs @@ -284,6 +299,7 @@ def __init__(self, settings: Settings) -> None: sym.maximum_velocity_control, ), apply_to_first_elements=True, + name=point.u_v.name() + "_bounds", ) # Bounds on contact force control inputs @@ -294,6 +310,7 @@ def __init__(self, settings: Settings) -> None: sym.maximum_force_control, ), apply_to_first_elements=True, + name=point.u_f.name() + "_bounds", ) # Creation of contact kinematics consistency functions @@ -318,6 +335,7 @@ def __init__(self, settings: Settings) -> None: problem.add_expression_to_horizon( expression=cs.MX(point.p == point_kinematics), apply_to_first_elements=False, + name=point.p.name() + "_kinematics_consistency", ) function_inputs["point_position_names"].append(point.p.name()) @@ -328,6 +346,7 @@ def __init__(self, settings: Settings) -> None: hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, x0=sym.kinematics.base.initial_position, integrator=default_integrator, + name="base_position_dynamics", ) # dot(q) = q_dot (base quaternion dynamics) @@ -336,6 +355,7 @@ def __init__(self, settings: Settings) -> None: == sym.kinematics.base.quaternion_velocity_xyzw, x0=sym.kinematics.base.initial_quaternion_xyzw, integrator=default_integrator, + name="base_quaternion_dynamics", ) # dot(s) = s_dot (joint position dynamics) @@ -343,6 +363,7 @@ def __init__(self, settings: Settings) -> None: hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, x0=sym.kinematics.joints.initial_positions, integrator=default_integrator, + name="joint_position_dynamics", ) # dot(com) = h_g[:3]/m (center of mass dynamics) @@ -351,6 +372,7 @@ def __init__(self, settings: Settings) -> None: hp.dot(sym.com) == com_dynamics, x0=sym.com_initial, integrator=default_integrator, + name="com_dynamics", ) # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) @@ -361,12 +383,14 @@ def __init__(self, settings: Settings) -> None: hp.dot(sym.centroidal_momentum) == centroidal_dynamics, x0=sym.centroidal_momentum_initial, integrator=default_integrator, + name="centroidal_momentum_dynamics", ) # Unitary quaternion problem.add_expression_to_horizon( expression=cs.MX(cs.sumsqr(sym.kinematics.base.quaternion_xyzw) == 1), apply_to_first_elements=False, + name="unitary_quaternion", ) # Consistency of com position with kinematics @@ -379,7 +403,9 @@ def __init__(self, settings: Settings) -> None: s=sym.kinematics.joints.positions, )["com_position"] problem.add_expression_to_horizon( - expression=cs.MX(sym.com == com_kinematics), apply_to_first_elements=False + expression=cs.MX(sym.com == com_kinematics), + apply_to_first_elements=False, + name="com_kinematics_consistency", ) # Consistency of centroidal momentum (angular part only) with kinematics @@ -397,6 +423,7 @@ def __init__(self, settings: Settings) -> None: problem.add_expression_to_horizon( expression=cs.MX(sym.centroidal_momentum[3:] == centroidal_kinematics[3:]), apply_to_first_elements=True, + name="centroidal_momentum_kinematics_consistency", ) def set_initial_conditions(self) -> None: # TODO: fill From f42c9adcf175db33368342edd68b980f1af7d0a1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 18:46:58 +0200 Subject: [PATCH 010/100] Added additional constraints - Max angular momentum - Min com height - Min lateral distance --- .../humanoid_walking_flat_ground/planner.py | 56 ++++++++++++++++++- 1 file changed, 55 insertions(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 39f62c71..d47569d5 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -48,6 +48,9 @@ class Settings: static_friction: float = dataclasses.field(default=None) maximum_velocity_control: np.ndarray = dataclasses.field(default=None) maximum_force_derivative: np.ndarray = dataclasses.field(default=None) + maximum_angular_momentum: float = dataclasses.field(default=None) + minimum_com_height: float = dataclasses.field(default=None) + minimum_feet_lateral_distance: float = dataclasses.field(default=None) casadi_function_options: dict = dataclasses.field(default_factory=dict) @@ -67,6 +70,9 @@ def __post_init__(self) -> None: self.static_friction = 0.3 self.maximum_velocity_control = np.ndarray([2.0, 2.0, 5.0]) self.maximum_force_derivative = np.ndarray([100.0, 100.0, 100.0]) + self.maximum_angular_momentum = 10.0 + self.minimum_com_height = 0.1 + self.minimum_feet_lateral_distance = 0.1 def is_valid(self) -> bool: return ( @@ -100,6 +106,11 @@ class Variables(hp.OptimizationObject): static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) maximum_velocity_control: hp.StorageType = hp.default_storage_field(hp.Parameter) maximum_force_derivative: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_angular_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_com_height: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_feet_lateral_distance: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -136,6 +147,9 @@ def __post_init__( self.static_friction = settings.static_friction self.maximum_velocity_control = settings.maximum_velocity_control self.maximum_force_derivative = settings.maximum_force_derivative + self.maximum_angular_momentum = settings.maximum_angular_momentum + self.minimum_com_height = settings.minimum_com_height + self.minimum_feet_lateral_distance = settings.minimum_feet_lateral_distance class HumanoidWalkingFlatGround: @@ -377,7 +391,8 @@ def __init__(self, settings: Settings) -> None: # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( - **function_inputs + number_of_points=len(function_inputs["point_position_names"]), + **function_inputs, ) problem.add_dynamics( hp.dot(sym.centroidal_momentum) == centroidal_dynamics, @@ -426,5 +441,44 @@ def __init__(self, settings: Settings) -> None: name="centroidal_momentum_kinematics_consistency", ) + # Bounds on angular momentum + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_angular_momentum, + sym.centroidal_momentum[3:], + sym.maximum_angular_momentum, + ), + apply_to_first_elements=True, + name="angular_momentum_bounds", + ) + + # Minimum com height + com_height = height_fun(p=sym.com)["point_height"] + problem.add_expression_to_horizon( + expression=cs.MX(com_height >= sym.minimum_com_height), + apply_to_first_elements=False, + name="minimum_com_height", + ) + + # Minimum feet lateral distance + left_frame = sym.contact_points.left[0].descriptor.foot_frame + right_frame = sym.contact_points.right[0].descriptor.foot_frame + relative_position_fun = hp_rp.frames_relative_position( + kindyn_object=self.kin_dyn_object, + reference_frame=right_frame, + target_frame=left_frame, + **function_inputs, + ) + relative_position = relative_position_fun(s=sym.kinematics.joints.positions)[ + "relative_position" + ] + problem.add_expression_to_horizon( + expression=cs.MX( + relative_position[:2] >= sym.minimum_feet_lateral_distance + ), + apply_to_first_elements=False, + name="minimum_feet_distance", + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 1dd062d684f11229f43307937b24fa64456334c1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 19:11:23 +0200 Subject: [PATCH 011/100] Added additional constraint for the maximum feet relative height --- .../humanoid_walking_flat_ground/planner.py | 36 +++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index d47569d5..eafb74fb 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -51,6 +51,7 @@ class Settings: maximum_angular_momentum: float = dataclasses.field(default=None) minimum_com_height: float = dataclasses.field(default=None) minimum_feet_lateral_distance: float = dataclasses.field(default=None) + maximum_feet_relative_height: float = dataclasses.field(default=None) casadi_function_options: dict = dataclasses.field(default_factory=dict) @@ -73,6 +74,7 @@ def __post_init__(self) -> None: self.maximum_angular_momentum = 10.0 self.minimum_com_height = 0.1 self.minimum_feet_lateral_distance = 0.1 + self.maximum_feet_relative_height = 0.05 def is_valid(self) -> bool: return ( @@ -111,6 +113,9 @@ class Variables(hp.OptimizationObject): minimum_feet_lateral_distance: hp.StorageType = hp.default_storage_field( hp.Parameter ) + maximum_feet_relative_height: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -150,6 +155,7 @@ def __post_init__( self.maximum_angular_momentum = settings.maximum_angular_momentum self.minimum_com_height = settings.minimum_com_height self.minimum_feet_lateral_distance = settings.minimum_feet_lateral_distance + self.maximum_feet_relative_height = settings.maximum_feet_relative_height class HumanoidWalkingFlatGround: @@ -480,5 +486,35 @@ def __init__(self, settings: Settings) -> None: name="minimum_feet_distance", ) + # Maximum feet relative height + def get_centroid( + points: list[ExtendedContactPoint], function_inputs_dict: dict + ) -> cs.MX: + function_inputs_dict["point_position_names"] = [ + pt.p.name() for pt in points + ] + point_position_dict = {pt.p.name(): pt.p for pt in points} + centroid_fun = hp_rp.contact_points_centroid( + number_of_points=len(function_inputs_dict["point_position_names"]), + **function_inputs_dict, + ) + return centroid_fun(**point_position_dict)["centroid"] + + left_centroid = get_centroid( + points=sym.contact_points.left, function_inputs_dict=function_inputs + ) + right_centroid = get_centroid( + points=sym.contact_points.right, function_inputs_dict=function_inputs + ) + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_feet_relative_height, + (left_centroid[2] - right_centroid[2]), + sym.maximum_feet_relative_height, + ), + apply_to_first_elements=False, + name="maximum_feet_relative_height", + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 8ba6715d7cc69f2f2e4327e74f346ef11faad336 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 10:32:51 +0200 Subject: [PATCH 012/100] Added bounds on joint positions and velocities --- .../humanoid_walking_flat_ground/planner.py | 49 +++++++++++++++++-- 1 file changed, 46 insertions(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index eafb74fb..6005927f 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -52,6 +52,10 @@ class Settings: minimum_com_height: float = dataclasses.field(default=None) minimum_feet_lateral_distance: float = dataclasses.field(default=None) maximum_feet_relative_height: float = dataclasses.field(default=None) + maximum_joint_positions: np.ndarray = dataclasses.field(default=None) + minimum_joint_positions: np.ndarray = dataclasses.field(default=None) + maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) + minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) casadi_function_options: dict = dataclasses.field(default_factory=dict) @@ -72,16 +76,25 @@ def __post_init__(self) -> None: self.maximum_velocity_control = np.ndarray([2.0, 2.0, 5.0]) self.maximum_force_derivative = np.ndarray([100.0, 100.0, 100.0]) self.maximum_angular_momentum = 10.0 - self.minimum_com_height = 0.1 - self.minimum_feet_lateral_distance = 0.1 - self.maximum_feet_relative_height = 0.05 def is_valid(self) -> bool: + number_of_joints = len(self.joints_name_list) return ( self.robot_urdf is not None and self.joints_name_list is not None and self.contact_points is not None and self.horizon_length is not None + and self.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 ) @@ -116,6 +129,10 @@ class Variables(hp.OptimizationObject): maximum_feet_relative_height: hp.StorageType = hp.default_storage_field( hp.Parameter ) + maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -156,6 +173,10 @@ def __post_init__( self.minimum_com_height = settings.minimum_com_height self.minimum_feet_lateral_distance = settings.minimum_feet_lateral_distance self.maximum_feet_relative_height = settings.maximum_feet_relative_height + self.maximum_joint_positions = settings.maximum_joint_positions + self.minimum_joint_positions = settings.minimum_joint_positions + self.maximum_joint_velocities = settings.maximum_joint_velocities + self.minimum_joint_velocities = settings.minimum_joint_velocities class HumanoidWalkingFlatGround: @@ -516,5 +537,27 @@ def get_centroid( name="maximum_feet_relative_height", ) + # Joint position bounds + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + sym.minimum_joint_positions, + sym.kinematics.joints.positions, + sym.maximum_joint_positions, + ), + apply_to_first_elements=False, + name="joint_position_bounds", + ) + + # Joint velocity bounds + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + sym.minimum_joint_velocities, + sym.kinematics.joints.velocities, + sym.maximum_joint_velocities, + ), + apply_to_first_elements=True, + name="joint_velocity_bounds", + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 08b67fb1f1577e9feb94561e74ba175e4072f543 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 11:45:25 +0200 Subject: [PATCH 013/100] Added contacts centroid cost --- .../humanoid_walking_flat_ground/planner.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 6005927f..59e0b445 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -57,6 +57,10 @@ class Settings: maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) + contacts_centroid_references: np.ndarray = dataclasses.field(default=None) + contacts_centroid_cost_weights: np.ndarray = dataclasses.field(default=None) + contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -95,6 +99,11 @@ def is_valid(self) -> bool: 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_references is not None + and len(self.contacts_centroid_references) == self.horizon_length + and self.contacts_centroid_cost_weights is not None + and len(self.contacts_centroid_cost_weights) == self.horizon_length + and self.contacts_centroid_cost_multiplier is not None ) @@ -134,6 +143,13 @@ class Variables(hp.OptimizationObject): maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) + contacts_centroid_references: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ adam.casadi.KinDynComputations @@ -177,6 +193,8 @@ def __post_init__( self.minimum_joint_positions = settings.minimum_joint_positions self.maximum_joint_velocities = settings.maximum_joint_velocities self.minimum_joint_velocities = settings.minimum_joint_velocities + self.contacts_centroid_references = settings.contacts_centroid_references + self.contacts_centroid_cost_weights = settings.contacts_centroid_cost_weights class HumanoidWalkingFlatGround: @@ -537,6 +555,23 @@ def get_centroid( name="maximum_feet_relative_height", ) + # Contact centroid position cost + centroid_error = sym.contacts_centroid_references - 0.5 * ( + left_centroid + right_centroid + ) + weighted_centroid_squared_error = ( + centroid_error.T() + @ cs.diag(sym.contacts_centroid_cost_weights) + @ centroid_error + ) + problem.add_expression_to_horizon( + expression=weighted_centroid_squared_error, + apply_to_first_elements=False, + name="contacts_centroid_cost", + mode=hp.ExpressionType.minimize, + scaling=self.settings.contacts_centroid_cost_multiplier, + ) + # Joint position bounds problem.add_expression_to_horizon( expression=cs.Opti_bounded( From 5c1a520b220d5dcdf896ce7846f938e128191a69 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 12:15:18 +0200 Subject: [PATCH 014/100] Separate the inputs needed to the centroidal dynamics --- .../humanoid_walking_flat_ground/planner.py | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 59e0b445..e526f1dc 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -279,8 +279,9 @@ def __init__(self, settings: Settings) -> None: ) point_kinematics_functions = {} + all_contact_points = sym.contact_points.left + sym.contact_points.right - for point in sym.contact_points.left + sym.contact_points.right: + for point in all_contact_points: # dot(f) = f_dot problem.add_dynamics( hp.dot(point.f) == point.f_dot, @@ -397,9 +398,6 @@ def __init__(self, settings: Settings) -> None: name=point.p.name() + "_kinematics_consistency", ) - function_inputs["point_position_names"].append(point.p.name()) - function_inputs["point_force_names"].append(point.f.name()) - # dot(pb) = pb_dot (base position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, @@ -435,6 +433,12 @@ def __init__(self, settings: Settings) -> None: ) # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) + function_inputs["point_position_names"] = [ + point.p.name() for point in all_contact_points + ] + function_inputs["point_force_names"] = [ + point.f.name() for point in all_contact_points + ] centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( number_of_points=len(function_inputs["point_position_names"]), **function_inputs, From e462c05826e01fea9a366cdba2d07d10f0156730 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 15:20:30 +0200 Subject: [PATCH 015/100] Refactoring of planner splitting the init into methods Added linear com velocity cost --- .../humanoid_walking_flat_ground/planner.py | 492 +++++++++++------- 1 file changed, 294 insertions(+), 198 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index e526f1dc..43b236a5 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -61,6 +61,10 @@ class Settings: contacts_centroid_cost_weights: np.ndarray = dataclasses.field(default=None) contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) + com_linear_velocity_reference: np.ndarray = dataclasses.field(default=None) + com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) + com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -104,6 +108,11 @@ def is_valid(self) -> bool: and self.contacts_centroid_cost_weights is not None and len(self.contacts_centroid_cost_weights) == self.horizon_length and self.contacts_centroid_cost_multiplier is not None + and self.com_linear_velocity_reference is not None + and self.com_linear_velocity_reference.shape == (3, self.horizon_length) + 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 ) @@ -150,6 +159,10 @@ class Variables(hp.OptimizationObject): hp.Parameter, time_dependent=True ) + com_linear_velocity_reference: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ adam.casadi.KinDynComputations @@ -218,11 +231,8 @@ def __init__(self, settings: Settings) -> None: self.variables, horizon_length=self.settings.horizon_length ) - problem = self.ocp.problem sym = self.ocp.symbolic_structure - default_integrator = self.settings.integrator - function_inputs = { "mass_name": sym.mass.name(), "momentum_name": sym.centroidal_momentum.name(), @@ -282,174 +292,92 @@ def __init__(self, settings: Settings) -> None: all_contact_points = sym.contact_points.left + sym.contact_points.right for point in all_contact_points: - # dot(f) = f_dot - problem.add_dynamics( - hp.dot(point.f) == point.f_dot, - x0=point.f0, - integrator=default_integrator, - name=point.f.name() + "_dynamics", + self.add_point_dynamics(point) + + self.add_contact_point_feasibility( + dcc_margin_fun, + dcc_planar_fun, + friction_margin_fun, + height_fun, + normal_force_fun, + point, ) - # dot(p) = v - problem.add_dynamics( - hp.dot(point.p) == point.v, - x0=point.p0, - integrator=default_integrator, - name=point.p.name() + "_dynamics", + self.add_contact_kinematic_consistency( + function_inputs, + normalized_quaternion, + point, + point_kinematics_functions, ) - # Planar complementarity - dcc_planar = dcc_planar_fun( - p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v - )["planar_complementarity"] - problem.add_expression_to_horizon( - expression=cs.MX(point.v == dcc_planar), - apply_to_first_elements=True, - name=point.p.name() + "_planar_complementarity", - ) + self.add_robot_dynamics(all_contact_points, function_inputs) - # Normal complementarity - dcc_margin = dcc_margin_fun( - p=point.p, - f=point.f, - v=point.v, - f_dot=point.f_dot, - k_bs=sym.dcc_gain, - eps=sym.dcc_epsilon, - )["dcc_complementarity_margin"] - problem.add_expression_to_horizon( - expression=cs.MX(dcc_margin >= 0), - apply_to_first_elements=True, - name=point.p.name() + "_dcc", - ) - - # Point height greater than zero - point_height = height_fun(p=point.p)["point_height"] - problem.add_expression_to_horizon( - expression=cs.MX(point_height >= 0), - apply_to_first_elements=False, - name=point.p.name() + "_height", - ) - - # Normal force greater than zero - normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] - problem.add_expression_to_horizon( - expression=cs.MX(normal_force >= 0), - apply_to_first_elements=False, - name=point.f.name() + "_normal", - ) - - # Friction - friction_margin = friction_margin_fun( - p=point.p, - f=point.f, - mu_s=sym.static_friction, - )["friction_cone_square_margin"] - problem.add_expression_to_horizon( - expression=cs.MX(friction_margin >= 0), - apply_to_first_elements=False, - name=point.f.name() + "_friction", - ) + self.add_kinematics_expressions( + function_inputs, height_fun, normalized_quaternion + ) - # Bounds on contact velocity control inputs - problem.add_expression_to_horizon( - expression=cs.Opti_bounded( - -sym.maximum_velocity_control, - point.u_v, - sym.maximum_velocity_control, - ), - apply_to_first_elements=True, - name=point.u_v.name() + "_bounds", - ) + self.add_contact_centroids_expressions(function_inputs) - # Bounds on contact force control inputs - problem.add_expression_to_horizon( - expression=cs.Opti_bounded( - -sym.maximum_force_control, - point.u_f, - sym.maximum_force_control, - ), - apply_to_first_elements=True, - name=point.u_f.name() + "_bounds", - ) + def add_contact_centroids_expressions(self, function_inputs): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure - # Creation of contact kinematics consistency functions - descriptor = point.descriptor - if descriptor.foot_frame not in point_kinematics_functions: - point_kinematics_functions[ - descriptor.foot_frame - ] = hp_rp.point_position_from_kinematics( - kindyn_object=self.kin_dyn_object, - frame_name=descriptor.foot_frame, - **function_inputs, - ) - - # Consistency between the contact position and the kinematics - point_kinematics = point_kinematics_functions[descriptor.foot_frame]( - pb=sym.kinematics.base.position, - qb=normalized_quaternion, - s=sym.kinematics.joints.positions, - p_parent=descriptor.position_in_foot_frame, - )["point_position"] - - problem.add_expression_to_horizon( - expression=cs.MX(point.p == point_kinematics), - apply_to_first_elements=False, - name=point.p.name() + "_kinematics_consistency", + # Maximum feet relative height + def get_centroid( + points: list[ExtendedContactPoint], function_inputs_dict: dict + ) -> cs.MX: + function_inputs_dict["point_position_names"] = [ + pt.p.name() for pt in points + ] + point_position_dict = {pt.p.name(): pt.p for pt in points} + centroid_fun = hp_rp.contact_points_centroid( + number_of_points=len(function_inputs_dict["point_position_names"]), + **function_inputs_dict, ) + return centroid_fun(**point_position_dict)["centroid"] - # dot(pb) = pb_dot (base position dynamics) - problem.add_dynamics( - hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, - x0=sym.kinematics.base.initial_position, - integrator=default_integrator, - name="base_position_dynamics", + left_centroid = get_centroid( + points=sym.contact_points.left, function_inputs_dict=function_inputs ) - - # dot(q) = q_dot (base quaternion dynamics) - problem.add_dynamics( - hp.dot(sym.kinematics.base.quaternion_xyzw) - == sym.kinematics.base.quaternion_velocity_xyzw, - x0=sym.kinematics.base.initial_quaternion_xyzw, - integrator=default_integrator, - name="base_quaternion_dynamics", + right_centroid = get_centroid( + points=sym.contact_points.right, function_inputs_dict=function_inputs ) - - # dot(s) = s_dot (joint position dynamics) - problem.add_dynamics( - hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, - x0=sym.kinematics.joints.initial_positions, - integrator=default_integrator, - name="joint_position_dynamics", + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + -sym.maximum_feet_relative_height, + (left_centroid[2] - right_centroid[2]), + sym.maximum_feet_relative_height, + ), + apply_to_first_elements=False, + name="maximum_feet_relative_height", ) - # dot(com) = h_g[:3]/m (center of mass dynamics) - com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) - problem.add_dynamics( - hp.dot(sym.com) == com_dynamics, - x0=sym.com_initial, - integrator=default_integrator, - name="com_dynamics", + # Contact centroid position cost + centroid_error = sym.contacts_centroid_references - 0.5 * ( + left_centroid + right_centroid ) - - # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) - function_inputs["point_position_names"] = [ - point.p.name() for point in all_contact_points - ] - function_inputs["point_force_names"] = [ - point.f.name() for point in all_contact_points - ] - centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( - number_of_points=len(function_inputs["point_position_names"]), - **function_inputs, + weighted_centroid_squared_error = ( + centroid_error.T() + @ cs.diag(sym.contacts_centroid_cost_weights) + @ centroid_error ) - problem.add_dynamics( - hp.dot(sym.centroidal_momentum) == centroidal_dynamics, - x0=sym.centroidal_momentum_initial, - integrator=default_integrator, - name="centroidal_momentum_dynamics", + problem.add_expression_to_horizon( + expression=weighted_centroid_squared_error, + apply_to_first_elements=False, + name="contacts_centroid_cost", + mode=hp.ExpressionType.minimize, + scaling=self.settings.contacts_centroid_cost_multiplier, ) + def add_kinematics_expressions( + self, + function_inputs: dict, + height_fun: cs.Function, + normalized_quaternion: cs.MX, + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure + # Unitary quaternion problem.add_expression_to_horizon( expression=cs.MX(cs.sumsqr(sym.kinematics.base.quaternion_xyzw) == 1), @@ -529,73 +457,241 @@ def __init__(self, settings: Settings) -> None: name="minimum_feet_distance", ) - # Maximum feet relative height - def get_centroid( - points: list[ExtendedContactPoint], function_inputs_dict: dict - ) -> cs.MX: - function_inputs_dict["point_position_names"] = [ - pt.p.name() for pt in points - ] - point_position_dict = {pt.p.name(): pt.p for pt in points} - centroid_fun = hp_rp.contact_points_centroid( - number_of_points=len(function_inputs_dict["point_position_names"]), - **function_inputs_dict, - ) - return centroid_fun(**point_position_dict)["centroid"] - - left_centroid = get_centroid( - points=sym.contact_points.left, function_inputs_dict=function_inputs - ) - right_centroid = get_centroid( - points=sym.contact_points.right, function_inputs_dict=function_inputs + # Joint position bounds + problem.add_expression_to_horizon( + expression=cs.Opti_bounded( + sym.minimum_joint_positions, + sym.kinematics.joints.positions, + sym.maximum_joint_positions, + ), + apply_to_first_elements=False, + name="joint_position_bounds", ) + + # Joint velocity bounds problem.add_expression_to_horizon( expression=cs.Opti_bounded( - -sym.maximum_feet_relative_height, - (left_centroid[2] - right_centroid[2]), - sym.maximum_feet_relative_height, + sym.minimum_joint_velocities, + sym.kinematics.joints.velocities, + sym.maximum_joint_velocities, ), + apply_to_first_elements=True, + name="joint_velocity_bounds", + ) + + # Desired com velocity + com_velocity_error = ( + sym.centroidal_momentum[:3] - sym.desired_com_velocity * sym.mass + ) + com_velocity_weighted_error = ( + com_velocity_error.T() + * cs.diag(cs.DM(self.settings.com_linear_velocity_cost_weights)) + * com_velocity_error + ) + problem.add_expression_to_horizon( + expression=com_velocity_weighted_error, + apply_to_first_elements=True, + name="com_velocity_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.com_linear_velocity_cost_multiplier, + ) + + def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure + default_integrator = self.settings.integrator + + # dot(pb) = pb_dot (base position dynamics) + problem.add_dynamics( + hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, + x0=sym.kinematics.base.initial_position, + integrator=default_integrator, + name="base_position_dynamics", + ) + + # dot(q) = q_dot (base quaternion dynamics) + problem.add_dynamics( + hp.dot(sym.kinematics.base.quaternion_xyzw) + == sym.kinematics.base.quaternion_velocity_xyzw, + x0=sym.kinematics.base.initial_quaternion_xyzw, + integrator=default_integrator, + name="base_quaternion_dynamics", + ) + + # dot(s) = s_dot (joint position dynamics) + problem.add_dynamics( + hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, + x0=sym.kinematics.joints.initial_positions, + integrator=default_integrator, + name="joint_position_dynamics", + ) + + # dot(com) = h_g[:3]/m (center of mass dynamics) + com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) + problem.add_dynamics( + hp.dot(sym.com) == com_dynamics, + x0=sym.com_initial, + integrator=default_integrator, + name="com_dynamics", + ) + + # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) + function_inputs["point_position_names"] = [ + point.p.name() for point in all_contact_points + ] + function_inputs["point_force_names"] = [ + point.f.name() for point in all_contact_points + ] + centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( + number_of_points=len(function_inputs["point_position_names"]), + **function_inputs, + ) + problem.add_dynamics( + hp.dot(sym.centroidal_momentum) == centroidal_dynamics, + x0=sym.centroidal_momentum_initial, + integrator=default_integrator, + name="centroidal_momentum_dynamics", + ) + + def add_contact_kinematic_consistency( + self, + function_inputs: dict, + normalized_quaternion: cs.MX, + point: ExtendedContactPoint, + point_kinematics_functions: dict, + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure + + # Creation of contact kinematics consistency functions + descriptor = point.descriptor + if descriptor.foot_frame not in point_kinematics_functions: + point_kinematics_functions[ + descriptor.foot_frame + ] = hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) + + # Consistency between the contact position and the kinematics + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=sym.kinematics.base.position, + qb=normalized_quaternion, + s=sym.kinematics.joints.positions, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] + problem.add_expression_to_horizon( + expression=cs.MX(point.p == point_kinematics), apply_to_first_elements=False, - name="maximum_feet_relative_height", + name=point.p.name() + "_kinematics_consistency", ) - # Contact centroid position cost - centroid_error = sym.contacts_centroid_references - 0.5 * ( - left_centroid + right_centroid + def add_contact_point_feasibility( + self, + dcc_margin_fun: cs.Function, + dcc_planar_fun: cs.Function, + friction_margin_fun: cs.Function, + height_fun: cs.Function, + normal_force_fun: cs.Function, + point: ExtendedContactPoint, + ): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure + + # Planar complementarity + dcc_planar = dcc_planar_fun( + p=point.p, kt=sym.planar_dcc_height_multiplier, u_p=point.u_v + )["planar_complementarity"] + problem.add_expression_to_horizon( + expression=cs.MX(point.v == dcc_planar), + apply_to_first_elements=True, + name=point.p.name() + "_planar_complementarity", + ) + + # Normal complementarity + dcc_margin = dcc_margin_fun( + p=point.p, + f=point.f, + v=point.v, + f_dot=point.f_dot, + k_bs=sym.dcc_gain, + eps=sym.dcc_epsilon, + )["dcc_complementarity_margin"] + problem.add_expression_to_horizon( + expression=cs.MX(dcc_margin >= 0), + apply_to_first_elements=True, + name=point.p.name() + "_dcc", ) - weighted_centroid_squared_error = ( - centroid_error.T() - @ cs.diag(sym.contacts_centroid_cost_weights) - @ centroid_error + + # Point height greater than zero + point_height = height_fun(p=point.p)["point_height"] + problem.add_expression_to_horizon( + expression=cs.MX(point_height >= 0), + apply_to_first_elements=False, + name=point.p.name() + "_height", ) + + # Normal force greater than zero + normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] problem.add_expression_to_horizon( - expression=weighted_centroid_squared_error, + expression=cs.MX(normal_force >= 0), apply_to_first_elements=False, - name="contacts_centroid_cost", - mode=hp.ExpressionType.minimize, - scaling=self.settings.contacts_centroid_cost_multiplier, + name=point.f.name() + "_normal", ) - # Joint position bounds + # Friction + friction_margin = friction_margin_fun( + p=point.p, + f=point.f, + mu_s=sym.static_friction, + )["friction_cone_square_margin"] + problem.add_expression_to_horizon( + expression=cs.MX(friction_margin >= 0), + apply_to_first_elements=False, + name=point.f.name() + "_friction", + ) + + # Bounds on contact velocity control inputs problem.add_expression_to_horizon( expression=cs.Opti_bounded( - sym.minimum_joint_positions, - sym.kinematics.joints.positions, - sym.maximum_joint_positions, + -sym.maximum_velocity_control, + point.u_v, + sym.maximum_velocity_control, ), - apply_to_first_elements=False, - name="joint_position_bounds", + apply_to_first_elements=True, + name=point.u_v.name() + "_bounds", # noqa ) - # Joint velocity bounds + # Bounds on contact force control inputs problem.add_expression_to_horizon( expression=cs.Opti_bounded( - sym.minimum_joint_velocities, - sym.kinematics.joints.velocities, - sym.maximum_joint_velocities, + -sym.maximum_force_control, + point.u_f, # noqa + sym.maximum_force_control, ), apply_to_first_elements=True, - name="joint_velocity_bounds", + name=point.u_f.name() + "_bounds", # noqa + ) + + def add_point_dynamics(self, point: ExtendedContactPoint) -> None: + default_integrator = self.settings.integrator + problem = self.ocp.problem + + # dot(f) = f_dot + problem.add_dynamics( + hp.dot(point.f) == point.f_dot, + x0=point.f0, + integrator=default_integrator, + name=point.f.name() + "_dynamics", + ) + + # dot(p) = v + problem.add_dynamics( + hp.dot(point.p) == point.v, + x0=point.p0, + integrator=default_integrator, + name=point.p.name() + "_dynamics", ) def set_initial_conditions(self) -> None: # TODO: fill From c1eaf6a09e0940353ecf123a30e7bb57bc3446f6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 16:00:37 +0200 Subject: [PATCH 016/100] Added frame regularization costs --- .../humanoid_walking_flat_ground/planner.py | 71 ++++++++++++++++++- 1 file changed, 68 insertions(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 43b236a5..5f240c5e 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -4,6 +4,7 @@ import adam.casadi import casadi as cs +import liecasadi import numpy as np import hippopt as hp @@ -65,6 +66,15 @@ class Settings: com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) + desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) + desired_frame_quaternion_xyzw_reference: np.ndarray = dataclasses.field( + default=None + ) + desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + base_quaternion_xyzw_reference: np.ndarray = dataclasses.field(default=None) + base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -109,10 +119,17 @@ def is_valid(self) -> bool: and len(self.contacts_centroid_cost_weights) == self.horizon_length and self.contacts_centroid_cost_multiplier is not None and self.com_linear_velocity_reference is not None - and self.com_linear_velocity_reference.shape == (3, self.horizon_length) + and len(self.com_linear_velocity_reference) == 3 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_xyzw_reference is not None + and len(self.desired_frame_quaternion_xyzw_reference) == 4 + and self.desired_frame_quaternion_cost_multiplier is not None + and self.base_quaternion_xyzw_reference is not None + and len(self.base_quaternion_xyzw_reference) == 4 + and self.base_quaternion_cost_multiplier is not None ) @@ -163,6 +180,14 @@ class Variables(hp.OptimizationObject): hp.Parameter, time_dependent=True ) + desired_frame_quaternion_xyzw_reference: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + + base_quaternion_xyzw_reference: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ adam.casadi.KinDynComputations @@ -208,6 +233,11 @@ def __post_init__( self.minimum_joint_velocities = settings.minimum_joint_velocities self.contacts_centroid_references = settings.contacts_centroid_references self.contacts_centroid_cost_weights = settings.contacts_centroid_cost_weights + self.com_linear_velocity_reference = settings.com_linear_velocity_reference + self.desired_frame_quaternion_xyzw_reference = ( + settings.desired_frame_quaternion_xyzw_reference + ) + self.base_quaternion_xyzw_reference = settings.base_quaternion_xyzw_reference class HumanoidWalkingFlatGround: @@ -257,6 +287,7 @@ def __init__(self, settings: Settings) -> None: "dcc_gain_name": "k_bs", "dcc_epsilon_name": "eps", "static_friction_name": "mu_s", + "desired_quaternion_xyzw_name": "qd", "options": self.settings.casadi_function_options, } @@ -312,9 +343,10 @@ def __init__(self, settings: Settings) -> None: self.add_robot_dynamics(all_contact_points, function_inputs) - self.add_kinematics_expressions( + self.add_kinematics_constraints( function_inputs, height_fun, normalized_quaternion ) + self.add_kinematics_regularization(function_inputs=function_inputs) self.add_contact_centroids_expressions(function_inputs) @@ -369,7 +401,7 @@ def get_centroid( scaling=self.settings.contacts_centroid_cost_multiplier, ) - def add_kinematics_expressions( + def add_kinematics_constraints( self, function_inputs: dict, height_fun: cs.Function, @@ -479,6 +511,9 @@ def add_kinematics_expressions( name="joint_velocity_bounds", ) + def add_kinematics_regularization(self, function_inputs: dict): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # Desired com velocity com_velocity_error = ( sym.centroidal_momentum[:3] - sym.desired_com_velocity * sym.mass @@ -496,6 +531,36 @@ def add_kinematics_expressions( scaling=self.settings.com_linear_velocity_cost_multiplier, ) + # Desired frame orientation + quaternion_error_fun = hp_rp.quaternion_error( + kindyn_object=self.kin_dyn_object, + target_frame=self.settings.desired_frame_quaternion_cost_frame_name, + **function_inputs, + ) + quaternion_error = quaternion_error_fun( + pb=sym.kinematics.base.position, + qb=sym.kinematics.base.quaternion_xyzw, + s=sym.kinematics.joints.positions, + qd=sym.desired_frame_quaternion_xyzw_reference, + )["quaternion_error"] + problem.add_expression_to_horizon( + expression=cs.sumsqr(quaternion_error), + apply_to_first_elements=False, + name="frame_quaternion_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.desired_frame_quaternion_cost_multiplier, + ) + + # Desired base orientation + identity = liecasadi.SO3.Identity() + problem.add_expression_to_horizon( + expression=cs.sumsqr(sym.kinematics.base.quaternion_xyzw - identity), + apply_to_first_elements=False, + name="base_quaternion_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.base_quaternion_cost_multiplier, + ) + def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem = self.ocp.problem sym = self.ocp.symbolic_structure From 5ce1ef13a73aecc40daac8b08e79a4c5ccce867d Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 17:17:14 +0200 Subject: [PATCH 017/100] Fixed used of initial conditions in planner --- .../humanoid_walking_flat_ground/planner.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 5f240c5e..a443c982 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -135,13 +135,13 @@ def is_valid(self) -> bool: @dataclasses.dataclass class Variables(hp.OptimizationObject): - contact_points: FeetContactPoints | list[ - FeetContactPoints - ] = hp.default_composite_field() + contact_points: hp.CompositeType[FeetContactPoints] = hp.default_composite_field() com: hp.StorageType = hp.default_storage_field(hp.Variable) centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) - kinematics: hp_rp.FloatingBaseSystem = hp.default_composite_field() + kinematics: hp.CompositeType[ + hp_rp.FloatingBaseSystem + ] = hp.default_composite_field() com_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) centroidal_momentum_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -569,7 +569,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(pb) = pb_dot (base position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, - x0=sym.kinematics.base.initial_position, + x0=problem.initial(sym.kinematics.base.initial_position), integrator=default_integrator, name="base_position_dynamics", ) @@ -578,7 +578,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.kinematics.base.quaternion_xyzw) == sym.kinematics.base.quaternion_velocity_xyzw, - x0=sym.kinematics.base.initial_quaternion_xyzw, + x0=problem.initial(sym.kinematics.base.initial_quaternion_xyzw), integrator=default_integrator, name="base_quaternion_dynamics", ) @@ -586,7 +586,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(s) = s_dot (joint position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, - x0=sym.kinematics.joints.initial_positions, + x0=problem.initial(sym.kinematics.joints.initial_positions), integrator=default_integrator, name="joint_position_dynamics", ) @@ -595,7 +595,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) problem.add_dynamics( hp.dot(sym.com) == com_dynamics, - x0=sym.com_initial, + x0=problem.initial(sym.com_initial), integrator=default_integrator, name="com_dynamics", ) @@ -613,7 +613,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): ) problem.add_dynamics( hp.dot(sym.centroidal_momentum) == centroidal_dynamics, - x0=sym.centroidal_momentum_initial, + x0=problem.initial(sym.centroidal_momentum_initial), integrator=default_integrator, name="centroidal_momentum_dynamics", ) @@ -746,7 +746,7 @@ def add_point_dynamics(self, point: ExtendedContactPoint) -> None: # dot(f) = f_dot problem.add_dynamics( hp.dot(point.f) == point.f_dot, - x0=point.f0, + x0=problem.initial(point.f0), integrator=default_integrator, name=point.f.name() + "_dynamics", ) @@ -754,7 +754,7 @@ def add_point_dynamics(self, point: ExtendedContactPoint) -> None: # dot(p) = v problem.add_dynamics( hp.dot(point.p) == point.v, - x0=point.p0, + x0=problem.initial(point.p0), integrator=default_integrator, name=point.p.name() + "_dynamics", ) From c0044f072c6d9227177cb90192440b35a0dfc300 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 9 Aug 2023 18:06:32 +0200 Subject: [PATCH 018/100] Added base quaternion velocity and joints regularization --- .../humanoid_walking_flat_ground/planner.py | 60 +++++++++++++++++++ 1 file changed, 60 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index a443c982..ee39fb6f 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -75,6 +75,15 @@ class Settings: base_quaternion_xyzw_reference: np.ndarray = dataclasses.field(default=None) base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + base_quaternion_xyzw_velocity_reference: np.ndarray = dataclasses.field( + default=None + ) + base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) + + joint_regularization_cost_reference: np.ndarray = dataclasses.field(default=None) + joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) + joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -130,6 +139,14 @@ def is_valid(self) -> bool: and self.base_quaternion_xyzw_reference is not None and len(self.base_quaternion_xyzw_reference) == 4 and self.base_quaternion_cost_multiplier is not None + and self.base_quaternion_xyzw_velocity_reference is not None + and len(self.base_quaternion_xyzw_velocity_reference) == 4 + and self.base_quaternion_velocity_cost_multiplier is not None + and self.joint_regularization_cost_reference is not None + and len(self.joint_regularization_cost_reference) == number_of_joints + 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 ) @@ -188,6 +205,14 @@ class Variables(hp.OptimizationObject): hp.Parameter, time_dependent=True ) + base_quaternion_xyzw_velocity_reference: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + + joint_regularization_cost_reference: hp.StorageType = hp.default_storage_field( + hp.Parameter, time_dependent=True + ) + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ adam.casadi.KinDynComputations @@ -238,6 +263,12 @@ def __post_init__( settings.desired_frame_quaternion_xyzw_reference ) self.base_quaternion_xyzw_reference = settings.base_quaternion_xyzw_reference + self.base_quaternion_xyzw_velocity_reference = ( + settings.base_quaternion_xyzw_velocity_reference + ) + self.joint_regularization_cost_reference = ( + settings.joint_regularization_cost_reference + ) class HumanoidWalkingFlatGround: @@ -561,6 +592,35 @@ def add_kinematics_regularization(self, function_inputs: dict): scaling=self.settings.base_quaternion_cost_multiplier, ) + # Desired base angular velocity + problem.add_expression_to_horizon( + expression=cs.sumsqr( + sym.kinematics.base.quaternion_velocity_xyzw + - sym.base_quaternion_xyzw_velocity_reference + ), + apply_to_first_elements=True, + name="base_quaternion_velocity_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.base_quaternion_velocity_cost_multiplier, + ) + + # Desired joint positions + joint_positions_error = ( + sym.kinematics.joints.positions - sym.joint_regularization_cost_reference + ) + joint_velocity_error = ( + sym.kinematics.joints.velocities + + cs.diag(cs.DM(self.settings.joint_regularization_cost_weights)) + * joint_positions_error + ) + problem.add_expression_to_horizon( + expression=cs.sumsqr(joint_velocity_error), + apply_to_first_elements=False, + name="joint_positions_error", + mode=hp.ExpressionType.minimize, + scaling=self.settings.joint_regularization_cost_multiplier, + ) + def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem = self.ocp.problem sym = self.ocp.symbolic_structure From 209fb6bdeb4f15522bb9c0a415605e53ce29d683 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 11:30:07 +0200 Subject: [PATCH 019/100] Added cost on force regularization --- .../humanoid_walking_flat_ground/planner.py | 49 +++++++++++++++++-- 1 file changed, 46 insertions(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index ee39fb6f..3c94c63a 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -16,9 +16,16 @@ class ExtendedContactPoint(hp_rp.ContactPoint): u_v: hp.StorageType = hp.default_storage_field(hp.Variable) - def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: + desired_ratio: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__( + self, input_descriptor: hp_rp.ContactPointDescriptor, number_of_points: int + ) -> None: super().__post_init__(input_descriptor) self.u_v = np.zeros(3) + self.desired_ratio = 1.0 / number_of_points @dataclasses.dataclass @@ -84,6 +91,8 @@ class Settings: joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + force_regularization_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -147,6 +156,7 @@ def is_valid(self) -> bool: 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 ) @@ -224,11 +234,15 @@ def __post_init__( kin_dyn_object: adam.casadi.KinDynComputations, ) -> None: self.contact_points.left = [ - hp_rp.ContactPoint(descriptor=point) + ExtendedContactPoint( + descriptor=point, number_of_points=len(settings.contact_points.left) + ) for point in settings.contact_points.left ] self.contact_points.right = [ - hp_rp.ContactPoint(descriptor=point) + ExtendedContactPoint( + descriptor=point, number_of_points=len(settings.contact_points.right) + ) for point in settings.contact_points.right ] @@ -381,6 +395,9 @@ def __init__(self, settings: Settings) -> None: self.add_contact_centroids_expressions(function_inputs) + self.add_foot_regularization(sym.contact_points.left) + self.add_foot_regularization(sym.contact_points.right) + def add_contact_centroids_expressions(self, function_inputs): problem = self.ocp.problem sym = self.ocp.symbolic_structure @@ -819,5 +836,31 @@ def add_point_dynamics(self, point: ExtendedContactPoint) -> None: name=point.p.name() + "_dynamics", ) + def add_foot_regularization(self, points: list[ExtendedContactPoint]) -> None: + problem = self.ocp.problem + + # Force ratio regularization + def sum_of_other_forces( + input_points: list[ExtendedContactPoint], point_index: int + ) -> cs.MX: + output_force = cs.MX.zeros(3, 1) + for i in range(len(input_points)): + if i != point_index: + output_force += input_points[i].f + return output_force + + for point in points: + force_error = point.f - point.desired_ratio * sum_of_other_forces( + points, points.index(point) + ) + + problem.add_expression_to_horizon( + expression=cs.sumsqr(force_error), + apply_to_first_elements=False, + name=point.f.name() + "_regularization", + mode=hp.ExpressionType.minimize, + scaling=self.settings.force_regularization_cost_multiplier, + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 2ab9b2c8b859c6bc2c1db1f12cb081ea6c46e2db Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 12:35:08 +0200 Subject: [PATCH 020/100] Moved references to dedicated structure --- .../humanoid_walking_flat_ground/planner.py | 114 +++++++----------- 1 file changed, 46 insertions(+), 68 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 3c94c63a..d131c128 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -65,29 +65,19 @@ class Settings: maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) - contacts_centroid_references: np.ndarray = dataclasses.field(default=None) - contacts_centroid_cost_weights: np.ndarray = dataclasses.field(default=None) contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) - com_linear_velocity_reference: np.ndarray = dataclasses.field(default=None) com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) - desired_frame_quaternion_xyzw_reference: np.ndarray = dataclasses.field( - default=None - ) + desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) - base_quaternion_xyzw_reference: np.ndarray = dataclasses.field(default=None) base_quaternion_cost_multiplier: float = dataclasses.field(default=None) - base_quaternion_xyzw_velocity_reference: np.ndarray = dataclasses.field( - default=None - ) base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) - joint_regularization_cost_reference: np.ndarray = dataclasses.field(default=None) joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) joint_regularization_cost_multiplier: float = dataclasses.field(default=None) @@ -131,28 +121,14 @@ def is_valid(self) -> bool: 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_references is not None - and len(self.contacts_centroid_references) == self.horizon_length - and self.contacts_centroid_cost_weights is not None - and len(self.contacts_centroid_cost_weights) == self.horizon_length and self.contacts_centroid_cost_multiplier is not None - and self.com_linear_velocity_reference is not None - and len(self.com_linear_velocity_reference) == 3 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_xyzw_reference is not None - and len(self.desired_frame_quaternion_xyzw_reference) == 4 and self.desired_frame_quaternion_cost_multiplier is not None - and self.base_quaternion_xyzw_reference is not None - and len(self.base_quaternion_xyzw_reference) == 4 and self.base_quaternion_cost_multiplier is not None - and self.base_quaternion_xyzw_velocity_reference is not None - and len(self.base_quaternion_xyzw_velocity_reference) == 4 and self.base_quaternion_velocity_cost_multiplier is not None - and self.joint_regularization_cost_reference is not None - and len(self.joint_regularization_cost_reference) == number_of_joints 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 @@ -160,6 +136,41 @@ def is_valid(self) -> bool: ) +@dataclasses.dataclass +class References(hp.OptimizationObject): + contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + contacts_centroid: hp.StorageType = hp.default_storage_field(hp.Parameter) + + com_linear_velocity: hp.StorageType = hp.default_storage_field(hp.Parameter) + + desired_frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + + base_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + base_quaternion_xyzw_velocity: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + + joint_regularization_cost: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_joints: int) -> None: + self.contacts_centroid_cost_weights = np.zeros((3, 1)) + self.contacts_centroid = np.zeros((3, 1)) + self.com_linear_velocity = np.zeros((3, 1)) + self.desired_frame_quaternion_xyzw = np.zeros((4, 1)) + self.desired_frame_quaternion_xyzw[3] = 1 + self.base_quaternion_xyzw = np.zeros((4, 1)) + self.base_quaternion_xyzw[3] = 1 + self.base_quaternion_xyzw_velocity = np.zeros((4, 1)) + self.joint_regularization_cost = np.zeros((number_of_joints, 1)) + + @dataclasses.dataclass class Variables(hp.OptimizationObject): contact_points: hp.CompositeType[FeetContactPoints] = hp.default_composite_field() @@ -196,32 +207,7 @@ class Variables(hp.OptimizationObject): maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) - contacts_centroid_references: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) - contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) - - com_linear_velocity_reference: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) - - desired_frame_quaternion_xyzw_reference: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) - - base_quaternion_xyzw_reference: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) - - base_quaternion_xyzw_velocity_reference: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) - - joint_regularization_cost_reference: hp.StorageType = hp.default_storage_field( - hp.Parameter, time_dependent=True - ) + references: hp.CompositeType[References] = hp.default_composite_field() settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -270,19 +256,7 @@ def __post_init__( self.minimum_joint_positions = settings.minimum_joint_positions self.maximum_joint_velocities = settings.maximum_joint_velocities self.minimum_joint_velocities = settings.minimum_joint_velocities - self.contacts_centroid_references = settings.contacts_centroid_references - self.contacts_centroid_cost_weights = settings.contacts_centroid_cost_weights - self.com_linear_velocity_reference = settings.com_linear_velocity_reference - self.desired_frame_quaternion_xyzw_reference = ( - settings.desired_frame_quaternion_xyzw_reference - ) - self.base_quaternion_xyzw_reference = settings.base_quaternion_xyzw_reference - self.base_quaternion_xyzw_velocity_reference = ( - settings.base_quaternion_xyzw_velocity_reference - ) - self.joint_regularization_cost_reference = ( - settings.joint_regularization_cost_reference - ) + self.references = References(number_of_joints=kin_dyn_object.NDoF) class HumanoidWalkingFlatGround: @@ -564,7 +538,7 @@ def add_kinematics_regularization(self, function_inputs: dict): sym = self.ocp.symbolic_structure # Desired com velocity com_velocity_error = ( - sym.centroidal_momentum[:3] - sym.desired_com_velocity * sym.mass + sym.centroidal_momentum[:3] - sym.references.com_linear_velocity * sym.mass ) com_velocity_weighted_error = ( com_velocity_error.T() @@ -589,7 +563,7 @@ def add_kinematics_regularization(self, function_inputs: dict): pb=sym.kinematics.base.position, qb=sym.kinematics.base.quaternion_xyzw, s=sym.kinematics.joints.positions, - qd=sym.desired_frame_quaternion_xyzw_reference, + qd=sym.references.desired_frame_quaternion_xyzw, )["quaternion_error"] problem.add_expression_to_horizon( expression=cs.sumsqr(quaternion_error), @@ -600,6 +574,7 @@ def add_kinematics_regularization(self, function_inputs: dict): ) # Desired base orientation + # TODO: use the actual reference identity = liecasadi.SO3.Identity() problem.add_expression_to_horizon( expression=cs.sumsqr(sym.kinematics.base.quaternion_xyzw - identity), @@ -613,7 +588,7 @@ def add_kinematics_regularization(self, function_inputs: dict): problem.add_expression_to_horizon( expression=cs.sumsqr( sym.kinematics.base.quaternion_velocity_xyzw - - sym.base_quaternion_xyzw_velocity_reference + - sym.references.base_quaternion_xyzw_velocity ), apply_to_first_elements=True, name="base_quaternion_velocity_error", @@ -623,7 +598,7 @@ def add_kinematics_regularization(self, function_inputs: dict): # Desired joint positions joint_positions_error = ( - sym.kinematics.joints.positions - sym.joint_regularization_cost_reference + sym.kinematics.joints.positions - sym.references.joint_regularization ) joint_velocity_error = ( sym.kinematics.joints.velocities @@ -864,3 +839,6 @@ def sum_of_other_forces( def set_initial_conditions(self) -> None: # TODO: fill pass + + def set_references(self, references: References) -> None: # TODO: fill + pass From cccff87d314bfe22bb331b3b0bde1dee894f1343 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 13:12:55 +0200 Subject: [PATCH 021/100] Fixed base quaternion cost --- .../humanoid_walking_flat_ground/planner.py | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index d131c128..93325101 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -554,19 +554,19 @@ def add_kinematics_regularization(self, function_inputs: dict): ) # Desired frame orientation - quaternion_error_fun = hp_rp.quaternion_error( + quaternion_error_kinematics_fun = hp_rp.quaternion_error_from_kinematics( kindyn_object=self.kin_dyn_object, target_frame=self.settings.desired_frame_quaternion_cost_frame_name, **function_inputs, ) - quaternion_error = quaternion_error_fun( + quaternion_error_kinematics = quaternion_error_kinematics_fun( pb=sym.kinematics.base.position, qb=sym.kinematics.base.quaternion_xyzw, s=sym.kinematics.joints.positions, qd=sym.references.desired_frame_quaternion_xyzw, )["quaternion_error"] problem.add_expression_to_horizon( - expression=cs.sumsqr(quaternion_error), + expression=cs.sumsqr(quaternion_error_kinematics), apply_to_first_elements=False, name="frame_quaternion_error", mode=hp.ExpressionType.minimize, @@ -574,10 +574,13 @@ def add_kinematics_regularization(self, function_inputs: dict): ) # Desired base orientation - # TODO: use the actual reference - identity = liecasadi.SO3.Identity() + quaternion_error_fun = hp_rp.quaternion_xyzw_error(**function_inputs) + quaternion_error = quaternion_error_fun( + q=sym.kinematics.base.quaternion_xyzw, + qd=sym.references.base_quaternion_xyzw, + )["quaternion_error"] problem.add_expression_to_horizon( - expression=cs.sumsqr(sym.kinematics.base.quaternion_xyzw - identity), + expression=cs.sumsqr(quaternion_error), apply_to_first_elements=False, name="base_quaternion_error", mode=hp.ExpressionType.minimize, From 3c92fedb74a289d3f33b2ad774e61b31f483a7b7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 19:04:50 +0200 Subject: [PATCH 022/100] Moved feet references to custom class and implemented yaw regularization task Renamed the yaw alignment function for consistency --- .../humanoid_walking_flat_ground/planner.py | 193 +++++++++++++++--- 1 file changed, 168 insertions(+), 25 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 93325101..69486b75 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -4,7 +4,6 @@ import adam.casadi import casadi as cs -import liecasadi import numpy as np import hippopt as hp @@ -16,16 +15,9 @@ class ExtendedContactPoint(hp_rp.ContactPoint): u_v: hp.StorageType = hp.default_storage_field(hp.Variable) - desired_ratio: hp.StorageType = hp.default_storage_field(hp.Parameter) - - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) - - def __post_init__( - self, input_descriptor: hp_rp.ContactPointDescriptor, number_of_points: int - ) -> None: + def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: super().__post_init__(input_descriptor) self.u_v = np.zeros(3) - self.desired_ratio = 1.0 / number_of_points @dataclasses.dataclass @@ -83,6 +75,8 @@ class Settings: force_regularization_cost_multiplier: float = dataclasses.field(default=None) + foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -133,11 +127,60 @@ def is_valid(self) -> bool: 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 ) +@dataclasses.dataclass +class ContactReferences(hp.OptimizationObject): + desired_force_ratio: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_points: int) -> None: + self.desired_force_ratio = 1.0 / number_of_points + + +@dataclasses.dataclass +class FootReferences(hp.OptimizationObject): + points: hp.CompositeType[list[ContactReferences]] = hp.default_composite_field( + time_varying=False + ) + yaw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_points: int) -> None: + self.points = [ + ContactReferences(number_of_points=number_of_points) + for _ in range(number_of_points) + ] + + self.yaw = 0.0 + + +@dataclasses.dataclass +class FeetReferences(hp.OptimizationObject): + left: hp.CompositeType[FootReferences] = hp.default_composite_field( + time_varying=False + ) + right: hp.CompositeType[FootReferences] = hp.default_composite_field( + time_varying=False + ) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_points: int) -> None: + self.left = FeetReferences(number_of_points=number_of_points) + self.right = FeetReferences(number_of_points=number_of_points) + + @dataclasses.dataclass class References(hp.OptimizationObject): + feet_references: hp.CompositeType[FeetReferences] = hp.default_composite_field( + time_varying=False + ) + contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( hp.Parameter ) @@ -158,8 +201,10 @@ class References(hp.OptimizationObject): joint_regularization_cost: hp.StorageType = hp.default_storage_field(hp.Parameter) number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) - def __post_init__(self, number_of_joints: int) -> None: + def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: + self.feet_references = FeetReferences(number_of_points=number_of_points) self.contacts_centroid_cost_weights = np.zeros((3, 1)) self.contacts_centroid = np.zeros((3, 1)) self.com_linear_velocity = np.zeros((3, 1)) @@ -220,15 +265,11 @@ def __post_init__( kin_dyn_object: adam.casadi.KinDynComputations, ) -> None: self.contact_points.left = [ - ExtendedContactPoint( - descriptor=point, number_of_points=len(settings.contact_points.left) - ) + ExtendedContactPoint(descriptor=point) for point in settings.contact_points.left ] self.contact_points.right = [ - ExtendedContactPoint( - descriptor=point, number_of_points=len(settings.contact_points.right) - ) + ExtendedContactPoint(descriptor=point) for point in settings.contact_points.right ] @@ -307,6 +348,9 @@ def __init__(self, settings: Settings) -> None: "dcc_epsilon_name": "eps", "static_friction_name": "mu_s", "desired_quaternion_xyzw_name": "qd", + "first_point_name": "p_0", + "second_point_name": "p_1", + "desired_yaw_name": "yd", "options": self.settings.casadi_function_options, } @@ -369,8 +413,20 @@ def __init__(self, settings: Settings) -> None: self.add_contact_centroids_expressions(function_inputs) - self.add_foot_regularization(sym.contact_points.left) - self.add_foot_regularization(sym.contact_points.right) + self.add_foot_regularization( + points=sym.contact_points.left, + descriptors=self.settings.contact_points.left, + references=sym.references.feet_references.left, + function_inputs=function_inputs, + foot_name="left", + ) + self.add_foot_regularization( + points=sym.contact_points.right, + descriptors=self.settings.contact_points.right, + references=sym.references.feet_references.right, + function_inputs=function_inputs, + foot_name="right", + ) def add_contact_centroids_expressions(self, function_inputs): problem = self.ocp.problem @@ -814,7 +870,14 @@ def add_point_dynamics(self, point: ExtendedContactPoint) -> None: name=point.p.name() + "_dynamics", ) - def add_foot_regularization(self, points: list[ExtendedContactPoint]) -> None: + def add_foot_regularization( + self, + points: list[ExtendedContactPoint], + descriptors: list[hp_rp.ContactPointDescriptor], + references: FootReferences, + function_inputs: dict, + foot_name: str, + ) -> None: problem = self.ocp.problem # Force ratio regularization @@ -822,14 +885,15 @@ def sum_of_other_forces( input_points: list[ExtendedContactPoint], point_index: int ) -> cs.MX: output_force = cs.MX.zeros(3, 1) - for i in range(len(input_points)): - if i != point_index: - output_force += input_points[i].f + for f in range(len(input_points)): + if f != point_index: + output_force += input_points[f].f return output_force - for point in points: - force_error = point.f - point.desired_ratio * sum_of_other_forces( - points, points.index(point) + for i, point in enumerate(points): + alpha = references.points[i].desired_force_ratio + force_error = point.f - alpha * sum_of_other_forces( + input_points=points, point_index=i ) problem.add_expression_to_horizon( @@ -840,6 +904,85 @@ def sum_of_other_forces( scaling=self.settings.force_regularization_cost_multiplier, ) + # Foot yaw task + centroid_position = np.zeros((3, 1)) + for descriptor in descriptors: + centroid_position += descriptor.position_in_foot_frame + centroid_position /= len(descriptors) + + bottom_right_index = None + top_right_index = None + top_left_index = None + + # The values below are useful to get the outermost points of the foot + bottom_right_value = 0 + top_right_value = 0 + top_left_value = 0 + + for i, descriptor in enumerate(descriptors): + relative_position = descriptor.position_in_foot_frame - centroid_position + + if ( + relative_position[1] < 0 + and relative_position[0] < 0 + and ( + bottom_right_index is None + or relative_position[0] * relative_position[1] > bottom_right_value + ) + ): + bottom_right_value = relative_position[0] * relative_position[1] + bottom_right_index = i + elif relative_position[1] < 0 < relative_position[0] and ( + top_right_index is None + or relative_position[0] * relative_position[1] > top_right_value + ): + top_right_value = relative_position[0] * relative_position[1] + top_right_index = i + elif ( + relative_position[1] > 0 + and relative_position[0] > 0 + and ( + top_left_index is None + or relative_position[0] * relative_position[1] > top_left_value + ) + ): + top_left_value = relative_position[0] * relative_position[1] + top_left_index = i + + assert bottom_right_index is not None + assert top_right_index is not None + assert top_left_index is not None + assert ( + bottom_right_index != top_right_index + and top_right_index != top_left_index + and top_left_index != bottom_right_index + ) + + yaw_alignment_fun = hp_rp.contact_points_yaw_alignment_error(**function_inputs) + yaw_alignment_forward = yaw_alignment_fun( + p_0=points[bottom_right_index].p, + p_1=points[top_right_index].p, + yd=references.yaw, + )["yaw_alignment_error"] + + yaw_alignment_sideways = yaw_alignment_fun( + p_0=points[top_right_index].p, + p_1=points[top_left_index].p, + yd=references.yaw + np.pi / 2, + )["yaw_alignment_error"] + + yaw_error = 0.5 * ( + cs.sumsqr(yaw_alignment_forward) + cs.sumsqr(yaw_alignment_sideways) + ) + + problem.add_expression_to_horizon( + expression=yaw_error, + apply_to_first_elements=False, + name=foot_name + "_yaw_regularization", + mode=hp.ExpressionType.minimize, + scaling=self.settings.foot_yaw_regularization_cost_multiplier, + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 4efcf1713c1959b8fff2792eeabd0e0a8f5fe062 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 11:48:51 +0200 Subject: [PATCH 023/100] Added contact point regularization --- .../humanoid_walking_flat_ground/planner.py | 73 +++++++++++++++++-- 1 file changed, 67 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 69486b75..14f1228d 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -77,6 +77,12 @@ class Settings: foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) + swing_foot_height_cost_multiplier: float = dataclasses.field(default=None) + + contact_velocity_control_cost_multiplier: float = dataclasses.field(default=None) + + contact_force_control_cost_multiplier: float = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) def __post_init__(self) -> None: @@ -128,6 +134,9 @@ def is_valid(self) -> bool: 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 ) @@ -168,16 +177,19 @@ class FeetReferences(hp.OptimizationObject): time_varying=False ) + desired_swing_height: hp.StorageType = hp.default_storage_field(hp.Parameter) + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_points: int) -> None: - self.left = FeetReferences(number_of_points=number_of_points) - self.right = FeetReferences(number_of_points=number_of_points) + self.left = FootReferences(number_of_points=number_of_points) + self.right = FootReferences(number_of_points=number_of_points) + self.desired_swing_height = 0.02 @dataclasses.dataclass class References(hp.OptimizationObject): - feet_references: hp.CompositeType[FeetReferences] = hp.default_composite_field( + feet: hp.CompositeType[FeetReferences] = hp.default_composite_field( time_varying=False ) @@ -204,7 +216,7 @@ class References(hp.OptimizationObject): number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: - self.feet_references = FeetReferences(number_of_points=number_of_points) + self.feet = FeetReferences(number_of_points=number_of_points) self.contacts_centroid_cost_weights = np.zeros((3, 1)) self.contacts_centroid = np.zeros((3, 1)) self.com_linear_velocity = np.zeros((3, 1)) @@ -351,6 +363,7 @@ def __init__(self, settings: Settings) -> None: "first_point_name": "p_0", "second_point_name": "p_1", "desired_yaw_name": "yd", + "desired_height_name": "hd", "options": self.settings.casadi_function_options, } @@ -404,6 +417,12 @@ def __init__(self, settings: Settings) -> None: point_kinematics_functions, ) + self.add_contact_point_regularization( + point=point, + desired_swing_height=sym.references.feet.desired_swing_height, + function_inputs=function_inputs, + ) + self.add_robot_dynamics(all_contact_points, function_inputs) self.add_kinematics_constraints( @@ -416,14 +435,14 @@ def __init__(self, settings: Settings) -> None: self.add_foot_regularization( points=sym.contact_points.left, descriptors=self.settings.contact_points.left, - references=sym.references.feet_references.left, + references=sym.references.feet.left, function_inputs=function_inputs, foot_name="left", ) self.add_foot_regularization( points=sym.contact_points.right, descriptors=self.settings.contact_points.right, - references=sym.references.feet_references.right, + references=sym.references.feet.right, function_inputs=function_inputs, foot_name="right", ) @@ -983,6 +1002,48 @@ def sum_of_other_forces( scaling=self.settings.foot_yaw_regularization_cost_multiplier, ) + def add_contact_point_regularization( + self, + point: ExtendedContactPoint, + desired_swing_height: hp.StorageType, + function_inputs: dict, + ) -> None: + problem = self.ocp.problem + + # Swing height regularization + swing_heuristic_fun = hp_rp.swing_height_heuristic( + self.settings.terrain, **function_inputs + ) + heuristic = swing_heuristic_fun(p=point.p, v=point.v, hd=desired_swing_height)[ + "heuristic" + ] + + problem.add_expression_to_horizon( + expression=heuristic, + apply_to_first_elements=False, + name=point.p.name() + "_swing_height_regularization", + mode=hp.ExpressionType.minimize, + scaling=self.settings.swing_foot_height_cost_multiplier, + ) + + # Contact velocity control regularization + problem.add_expression_to_horizon( + expression=cs.sumsqr(point.u_v), + apply_to_first_elements=False, + name=point.u_v.name() + "_regularization", # noqa + mode=hp.ExpressionType.minimize, + scaling=self.settings.contact_velocity_control_cost_multiplier, + ) + + # Contact force control regularization + problem.add_expression_to_horizon( + expression=cs.sumsqr(point.f_dot), + apply_to_first_elements=False, + name=point.f_dot.name() + "_regularization", # noqa + mode=hp.ExpressionType.minimize, + scaling=self.settings.contact_force_control_cost_multiplier, + ) + def set_initial_conditions(self) -> None: # TODO: fill pass From 57f5f06be8942074b09408b128359e77cb18a137 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 14:35:08 +0200 Subject: [PATCH 024/100] Implemented method to get/set the initial guess and set the references --- .../humanoid_walking_flat_ground/planner.py | 80 +++++++++++-------- 1 file changed, 45 insertions(+), 35 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 14f1228d..86ea5cf6 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -335,37 +335,7 @@ def __init__(self, settings: Settings) -> None: sym = self.ocp.symbolic_structure - function_inputs = { - "mass_name": sym.mass.name(), - "momentum_name": sym.centroidal_momentum.name(), - "com_name": sym.com.name(), - "quaternion_xyzw_name": "q", - "gravity_name": sym.gravity.name(), - "point_position_names": [], - "point_force_names": [], - "point_position_in_frame_name": "p_parent", - "base_position_name": "pb", - "base_quaternion_xyzw_name": "qb", - "joint_positions_name": "s", - "base_position_derivative_name": "pb_dot", - "base_quaternion_xyzw_derivative_name": "qb_dot", - "joint_velocities_name": "s_dot", - "point_position_name": "p", - "point_force_name": "f", - "point_velocity_name": "v", - "point_force_derivative_name": "f_dot", - "point_position_control_name": "u_p", - "height_multiplier_name": "kt", - "dcc_gain_name": "k_bs", - "dcc_epsilon_name": "eps", - "static_friction_name": "mu_s", - "desired_quaternion_xyzw_name": "qd", - "first_point_name": "p_0", - "second_point_name": "p_1", - "desired_yaw_name": "yd", - "desired_height_name": "hd", - "options": self.settings.casadi_function_options, - } + function_inputs = self.get_function_inputs_dict() # Normalized quaternion computation normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( @@ -447,6 +417,41 @@ def __init__(self, settings: Settings) -> None: foot_name="right", ) + def get_function_inputs_dict(self): + sym = self.ocp.symbolic_structure + function_inputs = { + "mass_name": sym.mass.name(), + "momentum_name": sym.centroidal_momentum.name(), + "com_name": sym.com.name(), + "quaternion_xyzw_name": "q", + "gravity_name": sym.gravity.name(), + "point_position_names": [], + "point_force_names": [], + "point_position_in_frame_name": "p_parent", + "base_position_name": "pb", + "base_quaternion_xyzw_name": "qb", + "joint_positions_name": "s", + "base_position_derivative_name": "pb_dot", + "base_quaternion_xyzw_derivative_name": "qb_dot", + "joint_velocities_name": "s_dot", + "point_position_name": "p", + "point_force_name": "f", + "point_velocity_name": "v", + "point_force_derivative_name": "f_dot", + "point_position_control_name": "u_p", + "height_multiplier_name": "kt", + "dcc_gain_name": "k_bs", + "dcc_epsilon_name": "eps", + "static_friction_name": "mu_s", + "desired_quaternion_xyzw_name": "qd", + "first_point_name": "p_0", + "second_point_name": "p_1", + "desired_yaw_name": "yd", + "desired_height_name": "hd", + "options": self.settings.casadi_function_options, + } + return function_inputs + def add_contact_centroids_expressions(self, function_inputs): problem = self.ocp.problem sym = self.ocp.symbolic_structure @@ -1044,8 +1049,13 @@ def add_contact_point_regularization( scaling=self.settings.contact_force_control_cost_multiplier, ) - def set_initial_conditions(self) -> None: # TODO: fill - pass + def set_initial_guess(self, initial_guess: Variables) -> None: + self.ocp.problem.set_initial_guess(initial_guess) + + def get_initial_guess(self) -> Variables: + return self.ocp.problem.get_initial_guess() - def set_references(self, references: References) -> None: # TODO: fill - pass + def set_references(self, references: References) -> None: + guess = self.get_initial_guess() + guess.references = references + self.set_initial_guess(guess) From 6aee68f242581e766adc945d073061d7117fe57c Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 15:19:07 +0200 Subject: [PATCH 025/100] Added possibility to set casadi options to opti in planner Added method to solve the problem --- .../humanoid_walking_flat_ground/planner.py | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 86ea5cf6..dec07884 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -85,12 +85,11 @@ class Settings: casadi_function_options: dict = dataclasses.field(default_factory=dict) + casadi_opti_options: dict = dataclasses.field(default_factory=dict) + + casadi_solver_options: dict = dataclasses.field(default_factory=dict) + def __post_init__(self) -> None: - self.casadi_function_options = ( - self.casadi_function_options - if isinstance(self.casadi_function_options, dict) - else {} - ) 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 @@ -329,8 +328,16 @@ def __init__(self, settings: Settings) -> None: settings=self.settings, kin_dyn_object=self.kin_dyn_object ) + optimization_solver = hp.OptiSolver( + options_solver=self.settings.casadi_solver_options, + options_plugin=self.settings.casadi_opti_options, + ) + ocp_solver = hp.MultipleShootingSolver(optimization_solver=optimization_solver) + self.ocp = hp.OptimalControlProblem.create( - self.variables, horizon_length=self.settings.horizon_length + self.variables, + optimal_control_solver=ocp_solver, + horizon_length=self.settings.horizon_length, ) sym = self.ocp.symbolic_structure @@ -1059,3 +1066,6 @@ def set_references(self, references: References) -> None: guess = self.get_initial_guess() guess.references = references self.set_initial_guess(guess) + + def solve(self) -> hp.Output[Variables]: + return self.ocp.problem.solve() From 069d0209ac12200b03b32240459669c73a6e22cc Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 17:15:56 +0200 Subject: [PATCH 026/100] Added dt to the settings Initial filling of the main file for the walking planner --- .../humanoid_walking_flat_ground/main.py | 75 +++++++++++++++++++ .../humanoid_walking_flat_ground/planner.py | 8 +- 2 files changed, 80 insertions(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index d59a411d..884cd963 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -1,4 +1,79 @@ +import argparse + import hippopt.turnkey_planners.humanoid_walking_flat_ground.planner as walking_planner if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Trajectory Optimization of a forward walking motion on ergoCub.", + ) + + parser.add_argument( + "--urdf", + type=str, + required=True, + help="Path to the ergoCubGazeboV1_minContacts URDF file.", + ) + planner_settings = walking_planner.Settings() + planner_settings.robot_urdf = parser.parse_args().urdf + planner_settings.joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", + ] + planner_settings.root_link = "root_link" + planner_settings.horizon_length = 20 + planner_settings.time_step = 0.1 + # planner_settings.contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) + + # planner_settings.planar_dcc_height_multiplier: float = dataclasses.field(default=None) + # planner_settings.dcc_gain: float = dataclasses.field(default=None) + # planner_settings.dcc_epsilon: float = dataclasses.field(default=None) + # planner_settings.static_friction: float = dataclasses.field(default=None) + # planner_settings.maximum_velocity_control: np.ndarray = dataclasses.field(default=None) + # planner_settings.maximum_force_derivative: np.ndarray = dataclasses.field(default=None) + # planner_settings.maximum_angular_momentum: float = dataclasses.field(default=None) + # planner_settings.minimum_com_height: float = dataclasses.field(default=None) + # planner_settings.minimum_feet_lateral_distance: float = dataclasses.field(default=None) + # planner_settings.maximum_feet_relative_height: float = dataclasses.field(default=None) + # planner_settings.maximum_joint_positions: np.ndarray = dataclasses.field(default=None) + # planner_settings.minimum_joint_positions: np.ndarray = dataclasses.field(default=None) + # planner_settings.maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) + # planner_settings.minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) + # planner_settings.contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) + # planner_settings.com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) + # planner_settings.desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) + # planner_settings.joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.force_regularization_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.swing_foot_height_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.contact_velocity_control_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.contact_force_control_cost_multiplier: float = dataclasses.field(default=None) + # planner_settings.casadi_function_options: dict = dataclasses.field(default_factory=dict) + # planner_settings.casadi_opti_options: dict = dataclasses.field(default_factory=dict) + # planner_settings.casadi_solver_options: dict = dataclasses.field(default_factory=dict) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index dec07884..d48c501c 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -40,6 +40,7 @@ class Settings: root_link: str = dataclasses.field(default=None) gravity: np.array = dataclasses.field(default=None) horizon_length: int = dataclasses.field(default=None) + time_step: float = dataclasses.field(default=None) integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) planar_dcc_height_multiplier: float = dataclasses.field(default=None) @@ -98,8 +99,8 @@ def __post_init__(self) -> None: self.dcc_gain = 20.0 self.dcc_epsilon = 0.05 self.static_friction = 0.3 - self.maximum_velocity_control = np.ndarray([2.0, 2.0, 5.0]) - self.maximum_force_derivative = np.ndarray([100.0, 100.0, 100.0]) + 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 def is_valid(self) -> bool: @@ -109,6 +110,7 @@ def is_valid(self) -> bool: 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 @@ -287,7 +289,7 @@ def __post_init__( self.com = np.zeros(3) self.centroidal_momentum = np.zeros(6) self.kinematics = hp_rp.FloatingBaseSystem(kin_dyn_object.NDoF) - self.dt = 0.1 + self.dt = settings.time_step self.gravity = kin_dyn_object.g[:, 3] self.mass = kin_dyn_object.get_total_mass() From 1129ec7c18cfa2b4516a23365c3de7d57fa19ae0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 18:28:43 +0200 Subject: [PATCH 027/100] Refactor variables to extract initial states --- .../humanoid_walking_flat_ground/planner.py | 70 ++++++++++++------- 1 file changed, 46 insertions(+), 24 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index d48c501c..7c6be3a1 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -12,7 +12,10 @@ @dataclasses.dataclass -class ExtendedContactPoint(hp_rp.ContactPoint): +class ExtendedContactPoint( + hp_rp.ContactPointState, + hp_rp.ContactPointStateDerivative, +): u_v: hp.StorageType = hp.default_storage_field(hp.Variable) def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: @@ -21,22 +24,16 @@ def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: @dataclasses.dataclass -class FeetContactPoints(hp.OptimizationObject): +class FeetContactPointsExtended(hp.OptimizationObject): left: list[ExtendedContactPoint] = hp.default_composite_field() right: list[ExtendedContactPoint] = hp.default_composite_field() -@dataclasses.dataclass -class FeetContactPointDescriptors: - left: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) - right: list[hp_rp.ContactPointDescriptor] = dataclasses.field(default_factory=list) - - @dataclasses.dataclass class Settings: robot_urdf: str = dataclasses.field(default=None) joints_name_list: list[str] = dataclasses.field(default=None) - contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) + contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) gravity: np.array = dataclasses.field(default=None) horizon_length: int = dataclasses.field(default=None) @@ -231,13 +228,21 @@ def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: @dataclasses.dataclass class Variables(hp.OptimizationObject): - contact_points: hp.CompositeType[FeetContactPoints] = hp.default_composite_field() + contact_points: hp.CompositeType[ + FeetContactPointsExtended + ] = hp.default_composite_field() + contact_points_initial_state: hp.CompositeType[ + hp_rp.FeetContactPoints + ] = hp.default_composite_field(time_varying=False) com: hp.StorageType = hp.default_storage_field(hp.Variable) centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) kinematics: hp.CompositeType[ hp_rp.FloatingBaseSystem ] = hp.default_composite_field() + kinematics_initial_state: hp.CompositeType[ + hp_rp.FloatingBaseSystemState + ] = hp.default_composite_field(time_varying=False) com_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) centroidal_momentum_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -285,10 +290,21 @@ def __post_init__( ExtendedContactPoint(descriptor=point) for point in settings.contact_points.right ] + self.contact_points_initial_state.left = [ + hp_rp.ContactPointState(descriptor=point) + for point in settings.contact_points.left + ] + self.contact_points_initial_state.right = [ + hp_rp.ContactPointState(descriptor=point) + for point in settings.contact_points.right + ] self.com = np.zeros(3) self.centroidal_momentum = np.zeros(6) self.kinematics = hp_rp.FloatingBaseSystem(kin_dyn_object.NDoF) + self.kinematics_initial_state = hp_rp.FloatingBaseSystemState( + kin_dyn_object.NDoF + ) self.dt = settings.time_step self.gravity = kin_dyn_object.g[:, 3] self.mass = kin_dyn_object.get_total_mass() @@ -342,7 +358,7 @@ def __init__(self, settings: Settings) -> None: horizon_length=self.settings.horizon_length, ) - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables function_inputs = self.get_function_inputs_dict() @@ -376,9 +392,13 @@ def __init__(self, settings: Settings) -> None: point_kinematics_functions = {} all_contact_points = sym.contact_points.left + sym.contact_points.right + all_contact_initial_state = ( + sym.contact_points_initial_state.left + + sym.contact_points_initial_state.right + ) - for point in all_contact_points: - self.add_point_dynamics(point) + for i, point in enumerate(all_contact_points): + self.add_point_dynamics(point, initial_state=all_contact_initial_state[i]) self.add_contact_point_feasibility( dcc_margin_fun, @@ -707,13 +727,13 @@ def add_kinematics_regularization(self, function_inputs: dict): def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem = self.ocp.problem - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables default_integrator = self.settings.integrator # dot(pb) = pb_dot (base position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, - x0=problem.initial(sym.kinematics.base.initial_position), + x0=problem.initial(sym.kinematics_initial_state.base.position), integrator=default_integrator, name="base_position_dynamics", ) @@ -722,7 +742,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.kinematics.base.quaternion_xyzw) == sym.kinematics.base.quaternion_velocity_xyzw, - x0=problem.initial(sym.kinematics.base.initial_quaternion_xyzw), + x0=problem.initial(sym.kinematics_initial_state.base.quaternion_xyzw), integrator=default_integrator, name="base_quaternion_dynamics", ) @@ -730,7 +750,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(s) = s_dot (joint position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, - x0=problem.initial(sym.kinematics.joints.initial_positions), + x0=problem.initial(sym.kinematics_initial_state.joints.positions), integrator=default_integrator, name="joint_position_dynamics", ) @@ -738,8 +758,8 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(com) = h_g[:3]/m (center of mass dynamics) com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) problem.add_dynamics( - hp.dot(sym.com) == com_dynamics, - x0=problem.initial(sym.com_initial), + hp.dot(sym.com) == com_dynamics, # noqa + x0=problem.initial(sym.com_initial), # noqa integrator=default_integrator, name="com_dynamics", ) @@ -756,8 +776,8 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): **function_inputs, ) problem.add_dynamics( - hp.dot(sym.centroidal_momentum) == centroidal_dynamics, - x0=problem.initial(sym.centroidal_momentum_initial), + hp.dot(sym.centroidal_momentum) == centroidal_dynamics, # noqa + x0=problem.initial(sym.centroidal_momentum_initial), # noqa integrator=default_integrator, name="centroidal_momentum_dynamics", ) @@ -883,14 +903,16 @@ def add_contact_point_feasibility( name=point.u_f.name() + "_bounds", # noqa ) - def add_point_dynamics(self, point: ExtendedContactPoint) -> None: + def add_point_dynamics( + self, point: ExtendedContactPoint, initial_state: hp_rp.ContactPointState + ) -> None: default_integrator = self.settings.integrator problem = self.ocp.problem # dot(f) = f_dot problem.add_dynamics( hp.dot(point.f) == point.f_dot, - x0=problem.initial(point.f0), + x0=problem.initial(initial_state.f), integrator=default_integrator, name=point.f.name() + "_dynamics", ) @@ -898,7 +920,7 @@ def add_point_dynamics(self, point: ExtendedContactPoint) -> None: # dot(p) = v problem.add_dynamics( hp.dot(point.p) == point.v, - x0=problem.initial(point.p0), + x0=problem.initial(initial_state.p), integrator=default_integrator, name=point.p.name() + "_dynamics", ) From f605e307cf424eb86c8c08eacb3a6d129edb1b9a Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 12:01:53 +0200 Subject: [PATCH 028/100] Moved initial state to dedicated dataclass --- .../humanoid_walking_flat_ground/planner.py | 84 ++++++++++++------- 1 file changed, 56 insertions(+), 28 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 7c6be3a1..16781438 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -226,26 +226,58 @@ def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: self.joint_regularization_cost = np.zeros((number_of_joints, 1)) +@dataclasses.dataclass +class InitialState(hp.OptimizationObject): + contact_points: hp.CompositeType[ + hp_rp.FeetContactPoints + ] = hp.default_composite_field(time_varying=False) + + kinematics: hp.CompositeType[ + hp_rp.FloatingBaseSystemState + ] = hp.default_composite_field(time_varying=False) + + com: hp.StorageType = hp.default_storage_field(hp.Parameter) + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) + + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) + kin_dyn_object: dataclasses.InitVar[ + adam.casadi.KinDynComputations + ] = dataclasses.field(default=None) + + def __post_init__( + self, + settings: Settings, + kin_dyn_object: adam.casadi.KinDynComputations, + ) -> None: + self.contact_points.left = [ + hp_rp.ContactPointState(descriptor=point) + for point in settings.contact_points.left + ] + self.contact_points.right = [ + hp_rp.ContactPointState(descriptor=point) + for point in settings.contact_points.right + ] + self.kinematics = hp_rp.FloatingBaseSystemState(kin_dyn_object.NDoF) + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(6) + + @dataclasses.dataclass class Variables(hp.OptimizationObject): contact_points: hp.CompositeType[ FeetContactPointsExtended ] = hp.default_composite_field() - contact_points_initial_state: hp.CompositeType[ - hp_rp.FeetContactPoints - ] = hp.default_composite_field(time_varying=False) + com: hp.StorageType = hp.default_storage_field(hp.Variable) centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) kinematics: hp.CompositeType[ hp_rp.FloatingBaseSystem ] = hp.default_composite_field() - kinematics_initial_state: hp.CompositeType[ - hp_rp.FloatingBaseSystemState - ] = hp.default_composite_field(time_varying=False) - com_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) - centroidal_momentum_initial: hp.StorageType = hp.default_storage_field(hp.Parameter) + initial_state: hp.CompositeType[InitialState] = hp.default_composite_field( + time_varying=False + ) dt: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -290,28 +322,19 @@ def __post_init__( ExtendedContactPoint(descriptor=point) for point in settings.contact_points.right ] - self.contact_points_initial_state.left = [ - hp_rp.ContactPointState(descriptor=point) - for point in settings.contact_points.left - ] - self.contact_points_initial_state.right = [ - hp_rp.ContactPointState(descriptor=point) - for point in settings.contact_points.right - ] self.com = np.zeros(3) self.centroidal_momentum = np.zeros(6) self.kinematics = hp_rp.FloatingBaseSystem(kin_dyn_object.NDoF) - self.kinematics_initial_state = hp_rp.FloatingBaseSystemState( - kin_dyn_object.NDoF + + self.initial_state = InitialState( + settings=settings, kin_dyn_object=kin_dyn_object ) + self.dt = settings.time_step self.gravity = kin_dyn_object.g[:, 3] self.mass = kin_dyn_object.get_total_mass() - self.com_initial = np.zeros(3) - self.centroidal_momentum_initial = np.zeros(6) - self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier self.dcc_gain = settings.dcc_gain self.dcc_epsilon = settings.dcc_epsilon @@ -393,8 +416,8 @@ def __init__(self, settings: Settings) -> None: point_kinematics_functions = {} all_contact_points = sym.contact_points.left + sym.contact_points.right all_contact_initial_state = ( - sym.contact_points_initial_state.left - + sym.contact_points_initial_state.right + sym.initial_state.contact_points.left + + sym.initial_state.contact_points.right ) for i, point in enumerate(all_contact_points): @@ -733,7 +756,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(pb) = pb_dot (base position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, - x0=problem.initial(sym.kinematics_initial_state.base.position), + x0=problem.initial(sym.initial_state.kinematics.base.position), integrator=default_integrator, name="base_position_dynamics", ) @@ -742,7 +765,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.kinematics.base.quaternion_xyzw) == sym.kinematics.base.quaternion_velocity_xyzw, - x0=problem.initial(sym.kinematics_initial_state.base.quaternion_xyzw), + x0=problem.initial(sym.initial_state.kinematics.base.quaternion_xyzw), integrator=default_integrator, name="base_quaternion_dynamics", ) @@ -750,7 +773,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(s) = s_dot (joint position dynamics) problem.add_dynamics( hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, - x0=problem.initial(sym.kinematics_initial_state.joints.positions), + x0=problem.initial(sym.initial_state.kinematics.joints.positions), integrator=default_integrator, name="joint_position_dynamics", ) @@ -759,7 +782,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) problem.add_dynamics( hp.dot(sym.com) == com_dynamics, # noqa - x0=problem.initial(sym.com_initial), # noqa + x0=problem.initial(sym.initial_state.com), # noqa integrator=default_integrator, name="com_dynamics", ) @@ -777,7 +800,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): ) problem.add_dynamics( hp.dot(sym.centroidal_momentum) == centroidal_dynamics, # noqa - x0=problem.initial(sym.centroidal_momentum_initial), # noqa + x0=problem.initial(sym.initial_state.centroidal_momentum), # noqa integrator=default_integrator, name="centroidal_momentum_dynamics", ) @@ -1091,5 +1114,10 @@ def set_references(self, references: References) -> None: guess.references = references self.set_initial_guess(guess) + def set_initial_state(self, initial_state: InitialState) -> None: + guess = self.get_initial_guess() + guess.initial_state = initial_state + self.set_initial_guess(guess) + def solve(self) -> hp.Output[Variables]: return self.ocp.problem.solve() From bfd5729a7094763e8faabbedd98c070483e6ab77 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 14:33:37 +0200 Subject: [PATCH 029/100] Improved definition of composite types Also, making sure to call the superclass __post_init__ --- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 16781438..86e29120 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -19,7 +19,8 @@ class ExtendedContactPoint( u_v: hp.StorageType = hp.default_storage_field(hp.Variable) def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: - super().__post_init__(input_descriptor) + hp_rp.ContactPointState.__post_init__(self, input_descriptor) + hp_rp.ContactPointStateDerivative.__post_init__(self) self.u_v = np.zeros(3) From dbc98c64206526e57ca0d143c1f730d5027f5cb1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 16:15:16 +0200 Subject: [PATCH 030/100] Specified that references are time varying Allow setting the references from a single point Allow setting the opti solver name and the problem type --- .../humanoid_walking_flat_ground/planner.py | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 86e29120..f66f5d23 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -82,6 +82,10 @@ class Settings: contact_force_control_cost_multiplier: float = dataclasses.field(default=None) + opti_solver: str = dataclasses.field(default="ipopt") + + problem_type: str = dataclasses.field(default="nlp") + casadi_function_options: dict = dataclasses.field(default_factory=dict) casadi_opti_options: dict = dataclasses.field(default_factory=dict) @@ -303,7 +307,9 @@ class Variables(hp.OptimizationObject): maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) - references: hp.CompositeType[References] = hp.default_composite_field() + references: hp.CompositeType[References] = hp.default_composite_field( + time_varying=True + ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) kin_dyn_object: dataclasses.InitVar[ @@ -371,6 +377,8 @@ def __init__(self, settings: Settings) -> None: ) optimization_solver = hp.OptiSolver( + inner_solver=self.settings.opti_solver, + problem_type=self.settings.problem_type, options_solver=self.settings.casadi_solver_options, options_plugin=self.settings.casadi_opti_options, ) @@ -1110,9 +1118,17 @@ def set_initial_guess(self, initial_guess: Variables) -> None: def get_initial_guess(self) -> Variables: return self.ocp.problem.get_initial_guess() - def set_references(self, references: References) -> None: + def set_references(self, references: References | list[References]) -> None: guess = self.get_initial_guess() - guess.references = references + + assert isinstance(guess.references, list) + if isinstance(references, list): + assert len(references) == len(guess.references) + for i in range(len(guess.references)): + guess.references[i] = references[i] + else: + for i in range(len(guess.references)): + guess.references[i] = references self.set_initial_guess(guess) def set_initial_state(self, initial_state: InitialState) -> None: From 9d99a2ee4bc3c26ebd993825f8503431bce48512 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 19:16:39 +0200 Subject: [PATCH 031/100] Some bugfixes (not all) on planner.py Finished specifying the settings. There are still several failures to be investigated. --- .../humanoid_walking_flat_ground/main.py | 109 +++++++++++++----- .../humanoid_walking_flat_ground/planner.py | 73 +++++++----- 2 files changed, 120 insertions(+), 62 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index 884cd963..81b5bcd0 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -1,5 +1,10 @@ import argparse +import casadi as cs +import idyntree.bindings as idyntree +import numpy as np + +import hippopt.robot_planning as hp_rp import hippopt.turnkey_planners.humanoid_walking_flat_ground.planner as walking_planner if __name__ == "__main__": @@ -41,39 +46,79 @@ "r_ankle_pitch", "r_ankle_roll", ] + number_of_joints = len(planner_settings.joints_name_list) + + idyntree_model_loader = idyntree.ModelLoader() + idyntree_model_loader.loadReducedModelFromFile( + planner_settings.robot_urdf, planner_settings.joints_name_list + ) + idyntree_model = idyntree_model_loader.model() + planner_settings.root_link = "root_link" planner_settings.horizon_length = 20 planner_settings.time_step = 0.1 - # planner_settings.contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) - # planner_settings.planar_dcc_height_multiplier: float = dataclasses.field(default=None) - # planner_settings.dcc_gain: float = dataclasses.field(default=None) - # planner_settings.dcc_epsilon: float = dataclasses.field(default=None) - # planner_settings.static_friction: float = dataclasses.field(default=None) - # planner_settings.maximum_velocity_control: np.ndarray = dataclasses.field(default=None) - # planner_settings.maximum_force_derivative: np.ndarray = dataclasses.field(default=None) - # planner_settings.maximum_angular_momentum: float = dataclasses.field(default=None) - # planner_settings.minimum_com_height: float = dataclasses.field(default=None) - # planner_settings.minimum_feet_lateral_distance: float = dataclasses.field(default=None) - # planner_settings.maximum_feet_relative_height: float = dataclasses.field(default=None) - # planner_settings.maximum_joint_positions: np.ndarray = dataclasses.field(default=None) - # planner_settings.minimum_joint_positions: np.ndarray = dataclasses.field(default=None) - # planner_settings.maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) - # planner_settings.minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) - # planner_settings.contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) - # planner_settings.com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) - # planner_settings.desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.base_quaternion_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) - # planner_settings.joint_regularization_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.force_regularization_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.swing_foot_height_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.contact_velocity_control_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.contact_force_control_cost_multiplier: float = dataclasses.field(default=None) - # planner_settings.casadi_function_options: dict = dataclasses.field(default_factory=dict) - # planner_settings.casadi_opti_options: dict = dataclasses.field(default_factory=dict) - # planner_settings.casadi_solver_options: dict = dataclasses.field(default_factory=dict) + planner_settings.contact_points = hp_rp.FeetContactPointDescriptors() + planner_settings.contact_points.left = ( + hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="l_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.5, 0.0]), + ) + ) + planner_settings.contact_points.right = ( + hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="r_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.5, 0.0]), + ) + ) + + planner_settings.planar_dcc_height_multiplier = 10.0 + planner_settings.dcc_gain = 20.0 + planner_settings.dcc_epsilon = 0.05 + planner_settings.static_friction = 0.3 + planner_settings.maximum_velocity_control = [2.0, 2.0, 5.0] + planner_settings.maximum_force_derivative = [100.0, 100.0, 100.0] + planner_settings.maximum_angular_momentum = 10.0 + planner_settings.minimum_com_height = 0.3 + planner_settings.minimum_feet_lateral_distance = 0.1 + planner_settings.maximum_feet_relative_height = 0.05 + + planner_settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) + planner_settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) + + for i in range(number_of_joints): + joint = idyntree_model.getJoint(i) + if joint.hasPosLimits(): + planner_settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) + planner_settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) + + planner_settings.maximum_joint_velocities = np.ones(number_of_joints) * 2.0 + planner_settings.minimum_joint_velocities = np.ones(number_of_joints) * -2.0 + + planner_settings.joint_regularization_cost_weights = np.ones(number_of_joints) + planner_settings.joint_regularization_cost_weights[:3] = 0.1 # torso + planner_settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + planner_settings.joint_regularization_cost_weights[11:] = 1.0 # legs + + planner_settings.contacts_centroid_cost_multiplier = 100.0 + planner_settings.com_linear_velocity_cost_weights = [10.0, 0.1, 1.0] + planner_settings.com_linear_velocity_cost_multiplier = 1.0 + planner_settings.desired_frame_quaternion_cost_frame_name = "chest" + planner_settings.desired_frame_quaternion_cost_multiplier = 90.0 + planner_settings.base_quaternion_cost_multiplier = 50.0 + planner_settings.base_quaternion_velocity_cost_multiplier = 0.001 + planner_settings.joint_regularization_cost_multiplier = 0.1 + planner_settings.force_regularization_cost_multiplier = 0.2 + planner_settings.foot_yaw_regularization_cost_multiplier = 2000.0 + planner_settings.swing_foot_height_cost_multiplier = 1000.0 + planner_settings.contact_velocity_control_cost_multiplier = 5.0 + planner_settings.contact_force_control_cost_multiplier = 0.0001 + planner_settings.casadi_function_options = {} + planner_settings.casadi_opti_options = {} + planner_settings.casadi_solver_options = {} + + planner = walking_planner.HumanoidWalkingFlatGround(settings=planner_settings) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index f66f5d23..f734508e 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -26,8 +26,8 @@ def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: @dataclasses.dataclass class FeetContactPointsExtended(hp.OptimizationObject): - left: list[ExtendedContactPoint] = hp.default_composite_field() - right: list[ExtendedContactPoint] = hp.default_composite_field() + left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) @dataclasses.dataclass @@ -150,19 +150,24 @@ class ContactReferences(hp.OptimizationObject): number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_points: int) -> None: - self.desired_force_ratio = 1.0 / number_of_points + self.desired_force_ratio = ( + 1.0 / number_of_points + if number_of_points is not None and number_of_points > 0 + else 0.0 + ) @dataclasses.dataclass class FootReferences(hp.OptimizationObject): points: hp.CompositeType[list[ContactReferences]] = hp.default_composite_field( - time_varying=False + factory=list, time_varying=False ) yaw: hp.StorageType = hp.default_storage_field(hp.Parameter) - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=0) def __post_init__(self, number_of_points: int) -> None: + number_of_points = number_of_points if number_of_points is not None else 0 self.points = [ ContactReferences(number_of_points=number_of_points) for _ in range(number_of_points) @@ -174,10 +179,10 @@ def __post_init__(self, number_of_points: int) -> None: @dataclasses.dataclass class FeetReferences(hp.OptimizationObject): left: hp.CompositeType[FootReferences] = hp.default_composite_field( - time_varying=False + factory=FootReferences, time_varying=False ) right: hp.CompositeType[FootReferences] = hp.default_composite_field( - time_varying=False + factory=FootReferences, time_varying=False ) desired_swing_height: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -193,7 +198,7 @@ def __post_init__(self, number_of_points: int) -> None: @dataclasses.dataclass class References(hp.OptimizationObject): feet: hp.CompositeType[FeetReferences] = hp.default_composite_field( - time_varying=False + factory=FeetReferences, time_varying=False ) contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( @@ -215,8 +220,8 @@ class References(hp.OptimizationObject): joint_regularization_cost: hp.StorageType = hp.default_storage_field(hp.Parameter) - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=0) def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: self.feet = FeetReferences(number_of_points=number_of_points) @@ -235,11 +240,13 @@ def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: class InitialState(hp.OptimizationObject): contact_points: hp.CompositeType[ hp_rp.FeetContactPoints - ] = hp.default_composite_field(time_varying=False) + ] = hp.default_composite_field(factory=hp_rp.FeetContactPoints, time_varying=False) kinematics: hp.CompositeType[ hp_rp.FloatingBaseSystemState - ] = hp.default_composite_field(time_varying=False) + ] = hp.default_composite_field( + factory=hp_rp.FloatingBaseSystemState, time_varying=False + ) com: hp.StorageType = hp.default_storage_field(hp.Parameter) centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -254,15 +261,21 @@ def __post_init__( settings: Settings, kin_dyn_object: adam.casadi.KinDynComputations, ) -> None: - self.contact_points.left = [ - hp_rp.ContactPointState(descriptor=point) - for point in settings.contact_points.left - ] - self.contact_points.right = [ - hp_rp.ContactPointState(descriptor=point) - for point in settings.contact_points.right - ] - self.kinematics = hp_rp.FloatingBaseSystemState(kin_dyn_object.NDoF) + if settings is not None: + self.contact_points.left = [ + hp_rp.ContactPointState(descriptor=point) + for point in settings.contact_points.left + ] + self.contact_points.right = [ + hp_rp.ContactPointState(descriptor=point) + for point in settings.contact_points.right + ] + if kin_dyn_object is not None: + self.kinematics = hp_rp.FloatingBaseSystemState( + number_of_joints_state=kin_dyn_object.NDoF + ) + else: + self.kinematics = hp_rp.FloatingBaseSystemState(number_of_joints_state=0) self.com = np.zeros(3) self.centroidal_momentum = np.zeros(6) @@ -271,17 +284,17 @@ def __post_init__( class Variables(hp.OptimizationObject): contact_points: hp.CompositeType[ FeetContactPointsExtended - ] = hp.default_composite_field() + ] = hp.default_composite_field(factory=FeetContactPointsExtended) com: hp.StorageType = hp.default_storage_field(hp.Variable) centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) - kinematics: hp.CompositeType[ - hp_rp.FloatingBaseSystem - ] = hp.default_composite_field() + kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( + factory=hp_rp.FloatingBaseSystem + ) initial_state: hp.CompositeType[InitialState] = hp.default_composite_field( - time_varying=False + factory=InitialState, time_varying=False ) dt: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -308,7 +321,7 @@ class Variables(hp.OptimizationObject): minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) references: hp.CompositeType[References] = hp.default_composite_field( - time_varying=True + factory=References, time_varying=True ) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) @@ -332,14 +345,14 @@ def __post_init__( self.com = np.zeros(3) self.centroidal_momentum = np.zeros(6) - self.kinematics = hp_rp.FloatingBaseSystem(kin_dyn_object.NDoF) + self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=kin_dyn_object.NDoF) self.initial_state = InitialState( settings=settings, kin_dyn_object=kin_dyn_object ) self.dt = settings.time_step - self.gravity = kin_dyn_object.g[:, 3] + self.gravity = kin_dyn_object.g[:3] self.mass = kin_dyn_object.get_total_mass() self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier @@ -387,7 +400,7 @@ def __init__(self, settings: Settings) -> None: self.ocp = hp.OptimalControlProblem.create( self.variables, optimal_control_solver=ocp_solver, - horizon_length=self.settings.horizon_length, + horizon=self.settings.horizon_length, ) sym = self.ocp.symbolic_structure # type: Variables From 89e598f973d2d7840dac242b53ad223cfb8cbb9a Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 17 Aug 2023 12:49:34 +0200 Subject: [PATCH 032/100] Fixed used of casadi transpose and horzcat --- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index f734508e..e6214654 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -565,7 +565,7 @@ def get_centroid( left_centroid + right_centroid ) weighted_centroid_squared_error = ( - centroid_error.T() + centroid_error.T @ cs.diag(sym.contacts_centroid_cost_weights) @ centroid_error ) @@ -695,7 +695,7 @@ def add_kinematics_regularization(self, function_inputs: dict): sym.centroidal_momentum[:3] - sym.references.com_linear_velocity * sym.mass ) com_velocity_weighted_error = ( - com_velocity_error.T() + com_velocity_error.T * cs.diag(cs.DM(self.settings.com_linear_velocity_cost_weights)) * com_velocity_error ) From b23356e323ba2d8fd4b9e608138aff856333a0d2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 17 Aug 2023 12:50:29 +0200 Subject: [PATCH 033/100] Fixed setting of feet references --- .../humanoid_walking_flat_ground/planner.py | 32 ++++++++++++++----- 1 file changed, 24 insertions(+), 8 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index e6214654..c3984721 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -187,11 +187,14 @@ class FeetReferences(hp.OptimizationObject): desired_swing_height: hp.StorageType = hp.default_storage_field(hp.Parameter) - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) - def __post_init__(self, number_of_points: int) -> None: - self.left = FootReferences(number_of_points=number_of_points) - self.right = FootReferences(number_of_points=number_of_points) + def __post_init__( + self, number_of_points_left: int, number_of_points_right: int + ) -> None: + self.left = FootReferences(number_of_points=number_of_points_left) + self.right = FootReferences(number_of_points=number_of_points_right) self.desired_swing_height = 0.02 @@ -221,10 +224,19 @@ class References(hp.OptimizationObject): joint_regularization_cost: hp.StorageType = hp.default_storage_field(hp.Parameter) number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=0) - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) - def __post_init__(self, number_of_joints: int, number_of_points: int) -> None: - self.feet = FeetReferences(number_of_points=number_of_points) + def __post_init__( + self, + number_of_joints: int, + number_of_points_left: int, + number_of_points_right: int, + ) -> None: + self.feet = FeetReferences( + number_of_points_left=number_of_points_left, + number_of_points_right=number_of_points_right, + ) self.contacts_centroid_cost_weights = np.zeros((3, 1)) self.contacts_centroid = np.zeros((3, 1)) self.com_linear_velocity = np.zeros((3, 1)) @@ -369,7 +381,11 @@ def __post_init__( self.minimum_joint_positions = settings.minimum_joint_positions self.maximum_joint_velocities = settings.maximum_joint_velocities self.minimum_joint_velocities = settings.minimum_joint_velocities - self.references = References(number_of_joints=kin_dyn_object.NDoF) + self.references = References( + number_of_joints=kin_dyn_object.NDoF, + number_of_points_left=len(settings.contact_points.left), + number_of_points_right=len(settings.contact_points.right), + ) class HumanoidWalkingFlatGround: From 687b16a87e9e9b9a80bf334554732b4587bfc891 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 17 Aug 2023 16:33:20 +0200 Subject: [PATCH 034/100] Bugfix on planner Using the correct names and input parameters Fixed some calculations involving row and column vectors Specifying the dt when adding dynamics --- .../humanoid_walking_flat_ground/planner.py | 45 ++++++++++++------- 1 file changed, 28 insertions(+), 17 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index c3984721..3403673d 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -221,7 +221,7 @@ class References(hp.OptimizationObject): hp.Parameter ) - joint_regularization_cost: hp.StorageType = hp.default_storage_field(hp.Parameter) + joint_regularization: hp.StorageType = hp.default_storage_field(hp.Parameter) number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=0) number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) @@ -245,7 +245,7 @@ def __post_init__( self.base_quaternion_xyzw = np.zeros((4, 1)) self.base_quaternion_xyzw[3] = 1 self.base_quaternion_xyzw_velocity = np.zeros((4, 1)) - self.joint_regularization_cost = np.zeros((number_of_joints, 1)) + self.joint_regularization = np.zeros((number_of_joints, 1)) @dataclasses.dataclass @@ -347,11 +347,11 @@ def __post_init__( kin_dyn_object: adam.casadi.KinDynComputations, ) -> None: self.contact_points.left = [ - ExtendedContactPoint(descriptor=point) + ExtendedContactPoint(input_descriptor=point) for point in settings.contact_points.left ] self.contact_points.right = [ - ExtendedContactPoint(descriptor=point) + ExtendedContactPoint(input_descriptor=point) for point in settings.contact_points.right ] @@ -364,7 +364,7 @@ def __post_init__( ) self.dt = settings.time_step - self.gravity = kin_dyn_object.g[:3] + self.gravity = kin_dyn_object.g self.mass = kin_dyn_object.get_total_mass() self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier @@ -414,7 +414,7 @@ def __init__(self, settings: Settings) -> None: ocp_solver = hp.MultipleShootingSolver(optimization_solver=optimization_solver) self.ocp = hp.OptimalControlProblem.create( - self.variables, + input_structure=self.variables, optimal_control_solver=ocp_solver, horizon=self.settings.horizon_length, ) @@ -577,12 +577,12 @@ def get_centroid( ) # Contact centroid position cost - centroid_error = sym.contacts_centroid_references - 0.5 * ( + centroid_error = sym.references.contacts_centroid - 0.5 * ( left_centroid + right_centroid ) weighted_centroid_squared_error = ( centroid_error.T - @ cs.diag(sym.contacts_centroid_cost_weights) + @ cs.diag(sym.references.contacts_centroid_cost_weights) @ centroid_error ) problem.add_expression_to_horizon( @@ -712,8 +712,8 @@ def add_kinematics_regularization(self, function_inputs: dict): ) com_velocity_weighted_error = ( com_velocity_error.T - * cs.diag(cs.DM(self.settings.com_linear_velocity_cost_weights)) - * com_velocity_error + @ cs.diag(cs.DM(self.settings.com_linear_velocity_cost_weights)) + @ com_velocity_error ) problem.add_expression_to_horizon( expression=com_velocity_weighted_error, @@ -795,6 +795,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, x0=problem.initial(sym.initial_state.kinematics.base.position), + dt=sym.dt, integrator=default_integrator, name="base_position_dynamics", ) @@ -804,6 +805,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): hp.dot(sym.kinematics.base.quaternion_xyzw) == sym.kinematics.base.quaternion_velocity_xyzw, x0=problem.initial(sym.initial_state.kinematics.base.quaternion_xyzw), + dt=sym.dt, integrator=default_integrator, name="base_quaternion_dynamics", ) @@ -812,6 +814,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, x0=problem.initial(sym.initial_state.kinematics.joints.positions), + dt=sym.dt, integrator=default_integrator, name="joint_position_dynamics", ) @@ -821,6 +824,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.com) == com_dynamics, # noqa x0=problem.initial(sym.initial_state.com), # noqa + dt=sym.dt, integrator=default_integrator, name="com_dynamics", ) @@ -839,6 +843,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem.add_dynamics( hp.dot(sym.centroidal_momentum) == centroidal_dynamics, # noqa x0=problem.initial(sym.initial_state.centroidal_momentum), # noqa + dt=sym.dt, integrator=default_integrator, name="centroidal_momentum_dynamics", ) @@ -887,7 +892,7 @@ def add_contact_point_feasibility( point: ExtendedContactPoint, ): problem = self.ocp.problem - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables # Planar complementarity dcc_planar = dcc_planar_fun( @@ -956,12 +961,12 @@ def add_contact_point_feasibility( # Bounds on contact force control inputs problem.add_expression_to_horizon( expression=cs.Opti_bounded( - -sym.maximum_force_control, - point.u_f, # noqa - sym.maximum_force_control, + -sym.maximum_force_derivative, + point.f_dot, + sym.maximum_force_derivative, ), apply_to_first_elements=True, - name=point.u_f.name() + "_bounds", # noqa + name=point.f_dot.name() + "_bounds", # noqa ) def add_point_dynamics( @@ -969,11 +974,13 @@ def add_point_dynamics( ) -> None: default_integrator = self.settings.integrator problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables # dot(f) = f_dot problem.add_dynamics( hp.dot(point.f) == point.f_dot, x0=problem.initial(initial_state.f), + dt=sym.dt, integrator=default_integrator, name=point.f.name() + "_dynamics", ) @@ -982,6 +989,7 @@ def add_point_dynamics( problem.add_dynamics( hp.dot(point.p) == point.v, x0=problem.initial(initial_state.p), + dt=sym.dt, integrator=default_integrator, name=point.p.name() + "_dynamics", ) @@ -1023,7 +1031,8 @@ def sum_of_other_forces( # Foot yaw task centroid_position = np.zeros((3, 1)) for descriptor in descriptors: - centroid_position += descriptor.position_in_foot_frame + position_in_foot_frame = descriptor.position_in_foot_frame.reshape((3, 1)) + centroid_position += position_in_foot_frame centroid_position /= len(descriptors) bottom_right_index = None @@ -1036,7 +1045,9 @@ def sum_of_other_forces( top_left_value = 0 for i, descriptor in enumerate(descriptors): - relative_position = descriptor.position_in_foot_frame - centroid_position + relative_position = ( + descriptor.position_in_foot_frame.reshape((3, 1)) - centroid_position + ) if ( relative_position[1] < 0 From 27ae02c6950b401dcb09d6eea4b238016dc965b2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 12 Sep 2023 19:53:18 +0200 Subject: [PATCH 035/100] Exploiting the OverridableVariable mechanism and added HumanoidState --- .../humanoid_walking_flat_ground/planner.py | 57 +++---------------- 1 file changed, 7 insertions(+), 50 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 3403673d..b4b4c426 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -248,50 +248,6 @@ def __post_init__( self.joint_regularization = np.zeros((number_of_joints, 1)) -@dataclasses.dataclass -class InitialState(hp.OptimizationObject): - contact_points: hp.CompositeType[ - hp_rp.FeetContactPoints - ] = hp.default_composite_field(factory=hp_rp.FeetContactPoints, time_varying=False) - - kinematics: hp.CompositeType[ - hp_rp.FloatingBaseSystemState - ] = hp.default_composite_field( - factory=hp_rp.FloatingBaseSystemState, time_varying=False - ) - - com: hp.StorageType = hp.default_storage_field(hp.Parameter) - centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) - - settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) - kin_dyn_object: dataclasses.InitVar[ - adam.casadi.KinDynComputations - ] = dataclasses.field(default=None) - - def __post_init__( - self, - settings: Settings, - kin_dyn_object: adam.casadi.KinDynComputations, - ) -> None: - if settings is not None: - self.contact_points.left = [ - hp_rp.ContactPointState(descriptor=point) - for point in settings.contact_points.left - ] - self.contact_points.right = [ - hp_rp.ContactPointState(descriptor=point) - for point in settings.contact_points.right - ] - if kin_dyn_object is not None: - self.kinematics = hp_rp.FloatingBaseSystemState( - number_of_joints_state=kin_dyn_object.NDoF - ) - else: - self.kinematics = hp_rp.FloatingBaseSystemState(number_of_joints_state=0) - self.com = np.zeros(3) - self.centroidal_momentum = np.zeros(6) - - @dataclasses.dataclass class Variables(hp.OptimizationObject): contact_points: hp.CompositeType[ @@ -302,11 +258,11 @@ class Variables(hp.OptimizationObject): centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( - factory=hp_rp.FloatingBaseSystem + cls=hp.Variable, factory=hp_rp.FloatingBaseSystem ) - initial_state: hp.CompositeType[InitialState] = hp.default_composite_field( - factory=InitialState, time_varying=False + initial_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False ) dt: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -359,8 +315,9 @@ def __post_init__( self.centroidal_momentum = np.zeros(6) self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=kin_dyn_object.NDoF) - self.initial_state = InitialState( - settings=settings, kin_dyn_object=kin_dyn_object + self.initial_state = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, ) self.dt = settings.time_step @@ -1171,7 +1128,7 @@ def set_references(self, references: References | list[References]) -> None: guess.references[i] = references self.set_initial_guess(guess) - def set_initial_state(self, initial_state: InitialState) -> None: + def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None: guess = self.get_initial_guess() guess.initial_state = initial_state self.set_initial_guess(guess) From 3448e7e4a0daa97e86f5517aabe42db13e5c6f5e Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 15:16:45 +0200 Subject: [PATCH 036/100] Fixed regularization of contact forces --- .../humanoid_walking_flat_ground/planner.py | 15 ++++----------- 1 file changed, 4 insertions(+), 11 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index b4b4c426..9032bf1c 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -962,20 +962,13 @@ def add_foot_regularization( problem = self.ocp.problem # Force ratio regularization - def sum_of_other_forces( - input_points: list[ExtendedContactPoint], point_index: int - ) -> cs.MX: - output_force = cs.MX.zeros(3, 1) - for f in range(len(input_points)): - if f != point_index: - output_force += input_points[f].f - return output_force + sum_of_forces = cs.MX.zeros(3, 1) + for point in points: + sum_of_forces += point.f for i, point in enumerate(points): alpha = references.points[i].desired_force_ratio - force_error = point.f - alpha * sum_of_other_forces( - input_points=points, point_index=i - ) + force_error = point.f - alpha * sum_of_forces problem.add_expression_to_horizon( expression=cs.sumsqr(force_error), From 4eb962383ad0976ff8d1923d66d3fed5eb254d9f Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 17:33:11 +0200 Subject: [PATCH 037/100] First draft of HumanoidPoseFinder --- .../humanoid_pose_finder/main.py | 103 ++++ .../humanoid_pose_finder/planner.py | 470 ++++++++++++++++++ 2 files changed, 573 insertions(+) create mode 100644 src/hippopt/turnkey_planners/humanoid_pose_finder/main.py create mode 100644 src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py new file mode 100644 index 00000000..e897a192 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -0,0 +1,103 @@ +import argparse + +import casadi as cs +import idyntree.bindings as idyntree +import numpy as np + +import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Trajectory Optimization of a forward walking motion on ergoCub.", + ) + + parser.add_argument( + "--urdf", + type=str, + required=True, + help="Path to the ergoCubGazeboV1_minContacts URDF file.", + ) + + planner_settings = pose_finder.Settings() + planner_settings.robot_urdf = parser.parse_args().urdf + planner_settings.joints_name_list = [ + "torso_pitch", + "torso_roll", + "torso_yaw", + "l_shoulder_pitch", + "l_shoulder_roll", + "l_shoulder_yaw", + "l_elbow", + "r_shoulder_pitch", + "r_shoulder_roll", + "r_shoulder_yaw", + "r_elbow", + "l_hip_pitch", + "l_hip_roll", + "l_hip_yaw", + "l_knee", + "l_ankle_pitch", + "l_ankle_roll", + "r_hip_pitch", + "r_hip_roll", + "r_hip_yaw", + "r_knee", + "r_ankle_pitch", + "r_ankle_roll", + ] + number_of_joints = len(planner_settings.joints_name_list) + + idyntree_model_loader = idyntree.ModelLoader() + idyntree_model_loader.loadReducedModelFromFile( + planner_settings.robot_urdf, planner_settings.joints_name_list + ) + idyntree_model = idyntree_model_loader.model() + + planner_settings.root_link = "root_link" + + planner_settings.contact_points = hp_rp.FeetContactPointDescriptors() + planner_settings.contact_points.left = ( + hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="l_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.5, 0.0]), + ) + ) + planner_settings.contact_points.right = ( + hp_rp.ContactPointDescriptor.rectangular_foot( + foot_frame="r_sole", + x_length=0.232, + y_length=0.1, + top_left_point_position=np.array([0.116, 0.5, 0.0]), + ) + ) + + planner_settings.relaxed_complementarity_epsilon = 0.0001 + planner_settings.static_friction = 0.3 + + planner_settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) + planner_settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) + + for i in range(number_of_joints): + joint = idyntree_model.getJoint(i) + if joint.hasPosLimits(): + planner_settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) + planner_settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) + + planner_settings.joint_regularization_cost_weights = np.ones(number_of_joints) + planner_settings.joint_regularization_cost_weights[:3] = 0.1 # torso + planner_settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + planner_settings.joint_regularization_cost_weights[11:] = 1.0 # legs + + planner_settings.base_quaternion_cost_multiplier = 50.0 + planner_settings.joint_regularization_cost_multiplier = 0.1 + planner_settings.force_regularization_cost_multiplier = 0.2 + planner_settings.average_force_regularization_cost_multiplier = 10.0 + planner_settings.point_position_regularization_cost_multiplier = 100.0 + planner_settings.casadi_function_options = {} + planner_settings.casadi_opti_options = {} + planner_settings.casadi_solver_options = {} + + planner = pose_finder.Planner(settings=planner_settings) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py new file mode 100644 index 00000000..5c43f802 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -0,0 +1,470 @@ +import copy +import dataclasses + +import adam.casadi +import casadi as cs +import numpy as np + +import hippopt as hp +import hippopt.robot_planning as hp_rp + + +@dataclasses.dataclass +class Settings: + robot_urdf: str = dataclasses.field(default=None) + joints_name_list: list[str] = dataclasses.field(default=None) + contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) + root_link: str = dataclasses.field(default=None) + gravity: np.array = dataclasses.field(default=None) + terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) + relaxed_complementarity_epsilon: float = dataclasses.field(default=None) + static_friction: float = dataclasses.field(default=None) + maximum_joint_positions: np.ndarray = dataclasses.field(default=None) + minimum_joint_positions: np.ndarray = dataclasses.field(default=None) + + base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) + joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + + force_regularization_cost_multiplier: float = dataclasses.field(default=None) + average_force_regularization_cost_multiplier: float = dataclasses.field( + default=None + ) + + point_position_regularization_cost_multiplier: float = dataclasses.field( + default=None + ) + + opti_solver: str = dataclasses.field(default="ipopt") + problem_type: str = dataclasses.field(default="nlp") + casadi_function_options: dict = dataclasses.field(default_factory=dict) + casadi_opti_options: dict = dataclasses.field(default_factory=dict) + casadi_solver_options: dict = dataclasses.field(default_factory=dict) + + def __post_init__(self) -> None: + self.root_link = "root_link" + self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) + self.terrain = hp_rp.PlanarTerrain() + self.relaxed_complementarity_epsilon = 0.0001 + self.static_friction = 0.3 + + def is_valid(self) -> bool: + return ( + self.robot_urdf is not None + and self.joints_name_list is not None + and self.contact_points is not None + and self.root_link is not None + and self.gravity is not None + and self.terrain is not None + and self.relaxed_complementarity_epsilon is not None + and self.static_friction is not None + and self.maximum_joint_positions is not None + and self.minimum_joint_positions is not None + and self.base_quaternion_cost_multiplier is not None + and self.joint_regularization_cost_weights is not None + and self.joint_regularization_cost_multiplier is not None + and self.force_regularization_cost_multiplier is not None + and self.average_force_regularization_cost_multiplier is not None + and self.point_position_regularization_cost_multiplier is not None + ) + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + state: hp_rp.HumanoidState = hp.default_composite_field( + cls=hp.Variable, factory=hp_rp.HumanoidState + ) + mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) + references: hp_rp.HumanoidState = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState + ) + + relaxed_complementarity_epsilon: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) + kin_dyn_object: dataclasses.InitVar[ + adam.casadi.KinDynComputations + ] = dataclasses.field(default=None) + + def __post_init__( + self, + settings: Settings, + kin_dyn_object: adam.casadi.KinDynComputations, + ): + self.state = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=len(settings.joints_name_list), + ) + self.references = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=len(settings.joints_name_list), + ) + self.mass = kin_dyn_object.get_total_mass() + self.gravity = settings.gravity + self.static_friction = settings.static_friction + self.relaxed_complementarity_epsilon = settings.relaxed_complementarity_epsilon + self.maximum_joint_positions = settings.maximum_joint_positions + self.minimum_joint_positions = settings.minimum_joint_positions + + +class Planner: + def __init__(self, settings: Settings): + if not settings.is_valid(): + raise ValueError("Settings are not valid") + + self.settings = copy.deepcopy(settings) + + self.kin_dyn_object = adam.casadi.KinDynComputations( + urdfstring=self.settings.robot_urdf, + joints_name_list=self.settings.joints_name_list, + root_link=self.settings.root_link, + gravity=self.settings.gravity, + f_opts=self.settings.casadi_function_options, + ) + structure = Variables( + settings=self.settings, kin_dyn_object=self.kin_dyn_object + ) + + optimization_solver = hp.OptiSolver( + inner_solver=self.settings.opti_solver, + problem_type=self.settings.problem_type, + options_solver=self.settings.casadi_solver_options, + options_plugin=self.settings.casadi_opti_options, + ) + + self.op = hp.OptimizationProblem.create( + input_structure=structure, optimization_solver=optimization_solver + ) + + variables = self.op.variables # type: Variables + + function_inputs = self.get_function_inputs_dict() + + # Normalized quaternion computation + normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( + **function_inputs + ) + normalized_quaternion = normalized_quaternion_fun( + q=variables.state.kinematics.base.quaternion_xyzw + )["quaternion_normalized"] + + # Align names used in the terrain function with those in function_inputs + self.settings.terrain.change_options(**function_inputs) + + # Definition of contact constraint functions + relaxed_complementarity_fun = hp_rp.relaxed_complementarity_margin( + terrain=self.settings.terrain, + **function_inputs, + ) + friction_margin_fun = hp_rp.friction_cone_square_margin( + terrain=self.settings.terrain, **function_inputs + ) + height_fun = self.settings.terrain.height_function() + normal_force_fun = hp_rp.normal_force_component( + terrain=self.settings.terrain, **function_inputs + ) + + point_kinematics_functions = {} + all_contact_points = ( + variables.state.contact_points.left + variables.state.contact_points.right + ) + + for i, point in enumerate(all_contact_points): + self.add_contact_point_feasibility( + relaxed_complementarity_fun, + friction_margin_fun, + height_fun, + normal_force_fun, + point, + ) + + self.add_contact_kinematic_consistency( + function_inputs, + normalized_quaternion, + point, + point_kinematics_functions, + ) + + self.add_kinematics_constraints( + function_inputs, normalized_quaternion, all_contact_points + ) + self.add_kinematics_regularization(function_inputs=function_inputs) + + self.add_foot_regularization( + points=variables.state.contact_points.left, + references=variables.references.contact_points.left, + ) + self.add_foot_regularization( + points=variables.state.contact_points.right, + references=variables.references.contact_points.right, + ) + + def get_function_inputs_dict(self): + variables = self.op.variables + function_inputs = { + "mass_name": variables.mass.name(), + "momentum_name": variables.state.centroidal_momentum.name(), + "com_name": variables.state.com.name(), + "quaternion_xyzw_name": "q", + "gravity_name": variables.gravity.name(), + "point_position_names": [], + "point_force_names": [], + "point_position_in_frame_name": "p_parent", + "base_position_name": "pb", + "base_quaternion_xyzw_name": "qb", + "joint_positions_name": "s", + "point_position_name": "p", + "point_force_name": "f", + "height_multiplier_name": "kt", + "relaxed_complementarity_epsilon_name": "eps", + "static_friction_name": "mu_s", + "desired_quaternion_xyzw_name": "qd", + "first_point_name": "p_0", + "second_point_name": "p_1", + "desired_yaw_name": "yd", + "desired_height_name": "hd", + "options": self.settings.casadi_function_options, + } + return function_inputs + + def add_kinematics_constraints( + self, + function_inputs: dict, + normalized_quaternion: cs.MX, + all_contact_points: list[hp_rp.ContactPointState], + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Unitary quaternion + problem.add_constraint( + expression=cs.MX( + cs.sumsqr(variables.state.kinematics.base.quaternion_xyzw) == 1 + ), + name="unitary_quaternion", + ) + + # Consistency of com position with kinematics + com_kinematics_fun = hp_rp.center_of_mass_position_from_kinematics( + kindyn_object=self.kin_dyn_object, **function_inputs + ) + com_kinematics = com_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + )["com_position"] + problem.add_constraint( + expression=cs.MX(variables.state.com == com_kinematics), + name="com_kinematics_consistency", + ) + + # Zero momentum derivative + # 0 = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) + function_inputs["point_position_names"] = [ + point.p.name() for point in all_contact_points + ] + function_inputs["point_force_names"] = [ + point.f.name() for point in all_contact_points + ] + centroidal_dynamics_fun = hp_rp.centroidal_dynamics_with_point_forces( + number_of_points=len(function_inputs["point_position_names"]), + **function_inputs, + ) + + centroidal_inputs = { + variables.mass.name(): variables.mass, # noqa + variables.gravity.name(): variables.gravity, # noqa + variables.state.com.name(): variables.state.com, + } + for point in all_contact_points: + centroidal_inputs[point.p.name()] = point.p + centroidal_inputs[point.f.name()] = point.f + + centroidal_dynamics = centroidal_dynamics_fun(**centroidal_inputs)["h_g_dot"] + + problem.add_constraint( + expression=centroidal_dynamics == np.zeros(6), # noqa + name="centroidal_momentum_dynamics", + ) + + # Joint position bounds + problem.add_constraint( + expression=cs.Opti_bounded( + variables.minimum_joint_positions, + variables.state.kinematics.joints.positions, + variables.maximum_joint_positions, + ), + name="joint_position_bounds", + ) + + def add_kinematics_regularization(self, function_inputs: dict): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Desired base orientation + quaternion_error_fun = hp_rp.quaternion_xyzw_error(**function_inputs) + quaternion_error = quaternion_error_fun( + q=variables.state.kinematics.base.quaternion_xyzw, + qd=variables.references.kinematics.base.quaternion_xyzw, + )["quaternion_error"] + problem.add_cost( + expression=cs.sumsqr(quaternion_error), + name="base_quaternion_error", + scaling=self.settings.base_quaternion_cost_multiplier, + ) + + # Desired joint positions + joint_positions_error = ( + variables.state.kinematics.joints.positions + - variables.references.kinematics.joints.positions + ) + + weighted_error = ( + joint_positions_error.T + @ cs.diag(self.settings.joint_regularization_cost_weights) + @ joint_positions_error + ) + + problem.add_cost( + expression=weighted_error, + name="joint_positions_error", + scaling=self.settings.joint_regularization_cost_multiplier, + ) + + def add_contact_kinematic_consistency( + self, + function_inputs: dict, + normalized_quaternion: cs.MX, + point: hp_rp.ContactPointState, + point_kinematics_functions: dict, + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Creation of contact kinematics consistency functions + descriptor = point.descriptor + if descriptor.foot_frame not in point_kinematics_functions: + point_kinematics_functions[ + descriptor.foot_frame + ] = hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) + + # Consistency between the contact position and the kinematics + point_kinematics = point_kinematics_functions[descriptor.foot_frame]( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + p_parent=descriptor.position_in_foot_frame, + )["point_position"] + problem.add_constraint( + expression=cs.MX(point.p == point_kinematics), + name=point.p.name() + "_kinematics_consistency", + ) + + def add_contact_point_feasibility( + self, + complementarity_margin_fun: cs.Function, + friction_margin_fun: cs.Function, + height_fun: cs.Function, + normal_force_fun: cs.Function, + point: hp_rp.ContactPointState, + ): + problem = self.op.problem + variables = self.op.variables # type: Variables + + # Normal complementarity + dcc_margin = complementarity_margin_fun( + p=point.p, + f=point.f, + eps=variables.relaxed_complementarity_epsilon, + )["relaxed_complementarity_margin"] + problem.add_constraint( + expression=cs.MX(dcc_margin >= 0), + name=point.p.name() + "_complementarity", + ) + + # Point height greater than zero + point_height = height_fun(p=point.p)["point_height"] + problem.add_constraint( + expression=cs.MX(point_height >= 0), + name=point.p.name() + "_height", + ) + + # Normal force greater than zero + normal_force = normal_force_fun(p=point.p, f=point.f)["normal_force"] + problem.add_constraint( + expression=cs.MX(normal_force >= 0), + name=point.f.name() + "_normal", + ) + + # Friction + friction_margin = friction_margin_fun( + p=point.p, + f=point.f, + mu_s=variables.static_friction, + )["friction_cone_square_margin"] + problem.add_constraint( + expression=cs.MX(friction_margin >= 0), + name=point.f.name() + "_friction", + ) + + def add_foot_regularization( + self, + points: list[hp_rp.ContactPointState], + references: list[hp_rp.ContactPointState], + ) -> None: + problem = self.op.problem + + assert len(points) > 0 + # Force ratio regularization + sum_of_forces = cs.MX.zeros(3, 1) + for point in points: + sum_of_forces += point.f + + multiplier = 1.0 / len(points) + + for point in points: + force_error_from_average = point.f - multiplier * sum_of_forces + + problem.add_cost( + expression=cs.sumsqr(force_error_from_average), + name=point.f.name() + "_average_regularization", + scaling=self.settings.average_force_regularization_cost_multiplier, + ) + + # Force and position regularization + for i, point in enumerate(points): + problem.add_cost( + expression=cs.sumsqr(point.p - references[i].p), + name=point.p.name() + "_regularization", + scaling=self.settings.point_position_regularization_cost_multiplier, + ) + problem.add_cost( + expression=cs.sumsqr(point.f - references[i].f), + name=point.f.name() + "_regularization", + scaling=self.settings.force_regularization_cost_multiplier, + ) + + def set_initial_guess(self, initial_guess: Variables) -> None: + self.op.problem.set_initial_guess(initial_guess) + + def get_initial_guess(self) -> Variables: + return self.op.problem.get_initial_guess() + + def set_references(self, references: hp_rp.HumanoidState) -> None: + guess = self.get_initial_guess() + guess.references = references + self.set_initial_guess(guess) + + def solve(self) -> hp.Output[hp_rp.HumanoidState]: + return self.op.problem.solve() From f47010e767b229809f933ac9b384b3ce6ecb82c2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 18:13:49 +0200 Subject: [PATCH 038/100] Added regularization of com position in pose finder --- .../turnkey_planners/humanoid_pose_finder/planner.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 5c43f802..7493c5db 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -24,6 +24,8 @@ class Settings: base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + com_regularization_cost_multiplier: float = dataclasses.field(default=None) + joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) joint_regularization_cost_multiplier: float = dataclasses.field(default=None) @@ -61,6 +63,7 @@ def is_valid(self) -> bool: and self.static_friction is not None and self.maximum_joint_positions is not None and self.minimum_joint_positions is not None + and self.com_regularization_cost_multiplier is not None and self.base_quaternion_cost_multiplier is not None and self.joint_regularization_cost_weights is not None and self.joint_regularization_cost_multiplier is not None @@ -320,6 +323,14 @@ def add_kinematics_regularization(self, function_inputs: dict): scaling=self.settings.base_quaternion_cost_multiplier, ) + # Desired center of mass position + com_position_error = variables.state.com - variables.references.com + problem.add_cost( + expression=cs.sumsqr(com_position_error), + name="com_position_error", + scaling=self.settings.com_regularization_cost_multiplier, + ) + # Desired joint positions joint_positions_error = ( variables.state.kinematics.joints.positions From e72b86a5dc15a444911f4bc7a7be2656791db01f Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 10 Oct 2023 13:03:42 +0200 Subject: [PATCH 039/100] Setting the references in the pose finder --- .../humanoid_pose_finder/main.py | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index e897a192..df1769e8 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -2,6 +2,7 @@ import casadi as cs import idyntree.bindings as idyntree +import liecasadi import numpy as np import hippopt.robot_planning as hp_rp @@ -94,6 +95,7 @@ planner_settings.base_quaternion_cost_multiplier = 50.0 planner_settings.joint_regularization_cost_multiplier = 0.1 planner_settings.force_regularization_cost_multiplier = 0.2 + planner_settings.com_regularization_cost_multiplier = 10.0 planner_settings.average_force_regularization_cost_multiplier = 10.0 planner_settings.point_position_regularization_cost_multiplier = 100.0 planner_settings.casadi_function_options = {} @@ -101,3 +103,60 @@ planner_settings.casadi_solver_options = {} planner = pose_finder.Planner(settings=planner_settings) + + references = hp_rp.HumanoidState( + contact_point_descriptors=planner_settings.contact_points, + number_of_joints=number_of_joints, + ) + + references.com = np.array([0.0, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + references.contact_points.left = hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + references.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + references.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + references.kinematics.joints.positions = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + + planner.set_references(references) From d1a31734f8db55aae65f56bc5f1536c63574c982 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 10 Oct 2023 18:57:42 +0200 Subject: [PATCH 040/100] The humanoid pose finder outputs something --- src/hippopt/turnkey_planners/humanoid_pose_finder/main.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index df1769e8..25dbc89c 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -1,4 +1,5 @@ import argparse +import logging import casadi as cs import idyntree.bindings as idyntree @@ -9,6 +10,8 @@ import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder if __name__ == "__main__": + logging.basicConfig(level=logging.DEBUG) + parser = argparse.ArgumentParser( description="Trajectory Optimization of a forward walking motion on ergoCub.", ) @@ -160,3 +163,5 @@ ) planner.set_references(references) + + output = planner.solve() From 83228c41e88bb30f4d4134a1190768fd405ab80b Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 13 Oct 2023 18:37:29 +0200 Subject: [PATCH 041/100] Added initial version of HumanStateVisualizer --- .../turnkey_planners/humanoid_pose_finder/main.py | 11 +++++++++++ 1 file changed, 11 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index 25dbc89c..4a69b3d4 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -165,3 +165,14 @@ planner.set_references(references) output = planner.solve() + + visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() + visualizer_settings.robot_model = planner_settings.robot_urdf + visualizer_settings.considered_joints = planner_settings.joints_name_list + visualizer_settings.terrain = planner_settings.terrain + visualizer_settings.working_folder = "./" + + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + + print("Press [Enter] to close.") + input() From 17afde8cff2391f71b0ef9d8f8251414c1c71a0c Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 13 Oct 2023 18:38:42 +0200 Subject: [PATCH 042/100] Added gitignore for stl and urdf files in humanoid_pose_finder --- src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore | 2 ++ 1 file changed, 2 insertions(+) create mode 100644 src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore b/src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore new file mode 100644 index 00000000..f48bb6bf --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/.gitignore @@ -0,0 +1,2 @@ +*.stl +*.urdf From d1338f0dbc2841264103fe8f94fb6db83142bc9d Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 14 Oct 2023 11:52:33 +0200 Subject: [PATCH 043/100] Fixed setting of contact points in foot frame --- src/hippopt/turnkey_planners/humanoid_pose_finder/main.py | 4 ++-- .../turnkey_planners/humanoid_walking_flat_ground/main.py | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index 4a69b3d4..fa7826ef 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -66,7 +66,7 @@ foot_frame="l_sole", x_length=0.232, y_length=0.1, - top_left_point_position=np.array([0.116, 0.5, 0.0]), + top_left_point_position=np.array([0.116, 0.05, 0.0]), ) ) planner_settings.contact_points.right = ( @@ -74,7 +74,7 @@ foot_frame="r_sole", x_length=0.232, y_length=0.1, - top_left_point_position=np.array([0.116, 0.5, 0.0]), + top_left_point_position=np.array([0.116, 0.05, 0.0]), ) ) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index 81b5bcd0..b6607296 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -64,7 +64,7 @@ foot_frame="l_sole", x_length=0.232, y_length=0.1, - top_left_point_position=np.array([0.116, 0.5, 0.0]), + top_left_point_position=np.array([0.116, 0.05, 0.0]), ) ) planner_settings.contact_points.right = ( @@ -72,7 +72,7 @@ foot_frame="r_sole", x_length=0.232, y_length=0.1, - top_left_point_position=np.array([0.116, 0.5, 0.0]), + top_left_point_position=np.array([0.116, 0.05, 0.0]), ) ) From 7c23e5528e7a1d9be7b8f9e12b449fe32e7f9408 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 14 Oct 2023 11:52:46 +0200 Subject: [PATCH 044/100] Added visualization of the HumanoidState --- src/hippopt/turnkey_planners/humanoid_pose_finder/main.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index fa7826ef..e282904e 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -169,10 +169,13 @@ visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() visualizer_settings.robot_model = planner_settings.robot_urdf visualizer_settings.considered_joints = planner_settings.joints_name_list + visualizer_settings.contact_points = planner_settings.contact_points visualizer_settings.terrain = planner_settings.terrain visualizer_settings.working_folder = "./" visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + output_var = output.values # type: pose_finder.Variables # noqa + visualizer.visualize(output_var.state) print("Press [Enter] to close.") input() From 7ae184f2877f20bcfcae787de4c636fd6d79a7c3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 15 Oct 2023 13:31:08 +0200 Subject: [PATCH 045/100] Adding rotation error in pose finder --- .../humanoid_pose_finder/main.py | 6 +- .../humanoid_pose_finder/planner.py | 133 ++++++++++++++---- .../humanoid_walking_flat_ground/planner.py | 19 ++- 3 files changed, 126 insertions(+), 32 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index e282904e..6121284e 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -59,6 +59,7 @@ idyntree_model = idyntree_model_loader.model() planner_settings.root_link = "root_link" + planner_settings.desired_frame_quaternion_cost_frame_name = "chest" planner_settings.contact_points = hp_rp.FeetContactPointDescriptors() planner_settings.contact_points.left = ( @@ -96,6 +97,7 @@ planner_settings.joint_regularization_cost_weights[11:] = 1.0 # legs planner_settings.base_quaternion_cost_multiplier = 50.0 + planner_settings.desired_frame_quaternion_cost_multiplier = 100.0 planner_settings.joint_regularization_cost_multiplier = 0.1 planner_settings.force_regularization_cost_multiplier = 0.2 planner_settings.com_regularization_cost_multiplier = 10.0 @@ -107,7 +109,7 @@ planner = pose_finder.Planner(settings=planner_settings) - references = hp_rp.HumanoidState( + references = pose_finder.References( contact_point_descriptors=planner_settings.contact_points, number_of_joints=number_of_joints, ) @@ -134,6 +136,8 @@ liecasadi.SO3.Identity().as_quat().coeffs() ) + references.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + references.kinematics.joints.positions = np.deg2rad( [ 7, diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 7493c5db..39cb9551 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -1,5 +1,6 @@ import copy import dataclasses +import logging import adam.casadi import casadi as cs @@ -15,6 +16,7 @@ class Settings: joints_name_list: list[str] = dataclasses.field(default=None) contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) root_link: str = dataclasses.field(default=None) + gravity: np.array = dataclasses.field(default=None) terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) relaxed_complementarity_epsilon: float = dataclasses.field(default=None) @@ -24,6 +26,10 @@ class Settings: base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) + + desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) + com_regularization_cost_multiplier: float = dataclasses.field(default=None) joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) @@ -52,25 +58,82 @@ def __post_init__(self) -> None: self.static_friction = 0.3 def is_valid(self) -> bool: - return ( - self.robot_urdf is not None - and self.joints_name_list is not None - and self.contact_points is not None - and self.root_link is not None - and self.gravity is not None - and self.terrain is not None - and self.relaxed_complementarity_epsilon is not None - and self.static_friction is not None - and self.maximum_joint_positions is not None - and self.minimum_joint_positions is not None - and self.com_regularization_cost_multiplier is not None - and self.base_quaternion_cost_multiplier is not None - and self.joint_regularization_cost_weights is not None - and self.joint_regularization_cost_multiplier is not None - and self.force_regularization_cost_multiplier is not None - and self.average_force_regularization_cost_multiplier is not None - and self.point_position_regularization_cost_multiplier is not None + ok = True + logger = logging.getLogger("[hippopt::HumanoidPoseFInder::Settings]") + if self.robot_urdf is None: + logger.error("robot_urdf is None") + ok = False + if self.joints_name_list is None: + logger.error("joints_name_list is None") + ok = False + if self.contact_points is None: + logger.error("contact_points is None") + ok = False + if self.root_link is None: + logger.error("root_link is None") + ok = False + if self.gravity is None: + logger.error("gravity is None") + ok = False + if self.terrain is None: + logger.error("terrain is None") + ok = False + if self.relaxed_complementarity_epsilon is None: + logger.error("relaxed_complementarity_epsilon is None") + ok = False + if self.static_friction is None: + logger.error("static_friction is None") + ok = False + if self.maximum_joint_positions is None: + logger.error("maximum_joint_positions is None") + ok = False + if self.minimum_joint_positions is None: + logger.error("minimum_joint_positions is None") + ok = False + if self.com_regularization_cost_multiplier is None: + logger.error("com_regularization_cost_multiplier is None") + ok = False + if self.base_quaternion_cost_multiplier is None: + logger.error("base_quaternion_cost_multiplier is None") + ok = False + if self.desired_frame_quaternion_cost_frame_name is None: + logger.error("desired_frame_quaternion_cost_frame_name is None") + ok = False + if self.desired_frame_quaternion_cost_multiplier is None: + logger.error("desired_frame_quaternion_cost_multiplier is None") + ok = False + if self.joint_regularization_cost_weights is None: + logger.error("joint_regularization_cost_weights is None") + ok = False + if self.joint_regularization_cost_multiplier is None: + logger.error("joint_regularization_cost_multiplier is None") + ok = False + if self.force_regularization_cost_multiplier is None: + logger.error("force_regularization_cost_multiplier is None") + ok = False + if self.average_force_regularization_cost_multiplier is None: + logger.error("average_force_regularization_cost_multiplier is None") + ok = False + if self.point_position_regularization_cost_multiplier is None: + logger.error("point_position_regularization_cost_multiplier is None") + ok = False + return ok + + +@dataclasses.dataclass +class References(hp_rp.HumanoidState): + frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ): + hp_rp.HumanoidState.__post_init__( + self, contact_point_descriptors, number_of_joints ) + self.frame_quaternion_xyzw = np.zeros(4) + self.frame_quaternion_xyzw[3] = 1.0 @dataclasses.dataclass @@ -80,8 +143,8 @@ class Variables(hp.OptimizationObject): ) mass: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) - references: hp_rp.HumanoidState = hp.default_composite_field( - cls=hp.Parameter, factory=hp_rp.HumanoidState + references: References = hp.default_composite_field( + cls=hp.Parameter, factory=References ) relaxed_complementarity_epsilon: hp.StorageType = hp.default_storage_field( @@ -105,7 +168,7 @@ def __post_init__( contact_point_descriptors=settings.contact_points, number_of_joints=len(settings.joints_name_list), ) - self.references = hp_rp.HumanoidState( + self.references = References( contact_point_descriptors=settings.contact_points, number_of_joints=len(settings.joints_name_list), ) @@ -198,7 +261,9 @@ def __init__(self, settings: Settings): self.add_kinematics_constraints( function_inputs, normalized_quaternion, all_contact_points ) - self.add_kinematics_regularization(function_inputs=function_inputs) + self.add_kinematics_regularization( + function_inputs=function_inputs, normalized_quaternion=normalized_quaternion + ) self.add_foot_regularization( points=variables.state.contact_points.left, @@ -307,7 +372,9 @@ def add_kinematics_constraints( name="joint_position_bounds", ) - def add_kinematics_regularization(self, function_inputs: dict): + def add_kinematics_regularization( + self, function_inputs: dict, normalized_quaternion: cs.MX + ): problem = self.op.problem variables = self.op.variables # type: Variables @@ -323,6 +390,24 @@ def add_kinematics_regularization(self, function_inputs: dict): scaling=self.settings.base_quaternion_cost_multiplier, ) + # Desired frame orientation + rotation_error_kinematics_fun = hp_rp.rotation_error_from_kinematics( + kindyn_object=self.kin_dyn_object, + target_frame=self.settings.desired_frame_quaternion_cost_frame_name, + **function_inputs, + ) + rotation_error_kinematics = rotation_error_kinematics_fun( + pb=variables.state.kinematics.base.position, + qb=normalized_quaternion, + s=variables.state.kinematics.joints.positions, + qd=variables.references.frame_quaternion_xyzw, + )["rotation_error"] + problem.add_cost( + expression=cs.sumsqr(cs.DM.eye(3) - rotation_error_kinematics), + name="frame_rotation_error", + scaling=self.settings.desired_frame_quaternion_cost_multiplier, + ) + # Desired center of mass position com_position_error = variables.state.com - variables.references.com problem.add_cost( @@ -472,7 +557,7 @@ def set_initial_guess(self, initial_guess: Variables) -> None: def get_initial_guess(self) -> Variables: return self.op.problem.get_initial_guess() - def set_references(self, references: hp_rp.HumanoidState) -> None: + def set_references(self, references: References) -> None: guess = self.get_initial_guess() guess.references = references self.set_initial_guess(guess) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 9032bf1c..526123e5 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -445,7 +445,10 @@ def __init__(self, settings: Settings) -> None: self.add_kinematics_constraints( function_inputs, height_fun, normalized_quaternion ) - self.add_kinematics_regularization(function_inputs=function_inputs) + self.add_kinematics_regularization( + function_inputs=function_inputs, + base_quaternion_normalized=normalized_quaternion, + ) self.add_contact_centroids_expressions(function_inputs) @@ -660,7 +663,9 @@ def add_kinematics_constraints( name="joint_velocity_bounds", ) - def add_kinematics_regularization(self, function_inputs: dict): + def add_kinematics_regularization( + self, function_inputs: dict, base_quaternion_normalized: cs.MX + ): problem = self.ocp.problem sym = self.ocp.symbolic_structure # Desired com velocity @@ -681,19 +686,19 @@ def add_kinematics_regularization(self, function_inputs: dict): ) # Desired frame orientation - quaternion_error_kinematics_fun = hp_rp.quaternion_error_from_kinematics( + rotation_error_kinematics_fun = hp_rp.rotation_error_from_kinematics( kindyn_object=self.kin_dyn_object, target_frame=self.settings.desired_frame_quaternion_cost_frame_name, **function_inputs, ) - quaternion_error_kinematics = quaternion_error_kinematics_fun( + rotation_error_kinematics = rotation_error_kinematics_fun( pb=sym.kinematics.base.position, - qb=sym.kinematics.base.quaternion_xyzw, + qb=base_quaternion_normalized, s=sym.kinematics.joints.positions, qd=sym.references.desired_frame_quaternion_xyzw, - )["quaternion_error"] + )["rotation_error"] problem.add_expression_to_horizon( - expression=cs.sumsqr(quaternion_error_kinematics), + expression=cs.sumsqr(cs.DM.eye(3) - rotation_error_kinematics), apply_to_first_elements=False, name="frame_quaternion_error", mode=hp.ExpressionType.minimize, From 1efd7c348f9bb3317ec95a02dae46e451424dac2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 10:19:01 +0200 Subject: [PATCH 046/100] Using the trace in the rotation error. --- src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py | 2 +- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 39cb9551..77f61478 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -403,7 +403,7 @@ def add_kinematics_regularization( qd=variables.references.frame_quaternion_xyzw, )["rotation_error"] problem.add_cost( - expression=cs.sumsqr(cs.DM.eye(3) - rotation_error_kinematics), + expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), name="frame_rotation_error", scaling=self.settings.desired_frame_quaternion_cost_multiplier, ) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 526123e5..985dc848 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -698,7 +698,7 @@ def add_kinematics_regularization( qd=sym.references.desired_frame_quaternion_xyzw, )["rotation_error"] problem.add_expression_to_horizon( - expression=cs.sumsqr(cs.DM.eye(3) - rotation_error_kinematics), + expression=cs.sumsqr(cs.trace(rotation_error_kinematics) - 3), apply_to_first_elements=False, name="frame_quaternion_error", mode=hp.ExpressionType.minimize, From 1e3dd5fb7b15370b46719e5d6c5da99411d45b1e Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 11:53:25 +0200 Subject: [PATCH 047/100] Adding visualization of two states --- .../humanoid_pose_finder/main.py | 29 +++++++++++++++++-- 1 file changed, 27 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index 6121284e..accb3e32 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -178,8 +178,33 @@ visualizer_settings.working_folder = "./" visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) - output_var = output.values # type: pose_finder.Variables # noqa - visualizer.visualize(output_var.state) + visualizer.visualize(output.values.state) # noqa + + print("Press [Enter] to move to the compute the next pose.") + input() + + references.com = np.array([0.15, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + references.contact_points.left = hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + references.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + planner.set_references(references) + + output = planner.solve() + visualizer.visualize(output.values.state) # noqa print("Press [Enter] to close.") input() From 363b32a515ce4174f15a9d687fe88df9ad53a1b5 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 12:01:39 +0200 Subject: [PATCH 048/100] Renamed HumanoidWalkingFlatGround to Planner --- .../turnkey_planners/humanoid_walking_flat_ground/main.py | 2 +- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index b6607296..ff322470 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -121,4 +121,4 @@ planner_settings.casadi_opti_options = {} planner_settings.casadi_solver_options = {} - planner = walking_planner.HumanoidWalkingFlatGround(settings=planner_settings) + planner = walking_planner.Planner(settings=planner_settings) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 985dc848..1803d249 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -345,7 +345,7 @@ def __post_init__( ) -class HumanoidWalkingFlatGround: +class Planner: def __init__(self, settings: Settings) -> None: if not settings.is_valid(): raise ValueError("Settings are not valid") From 3bf5a63dca3dc6a2c0e1ec1fbbf3da8ae3103bc4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 15:18:27 +0200 Subject: [PATCH 049/100] The reference for the pose finder contains HumanoidState instead of inheriting --- .../humanoid_pose_finder/main.py | 26 +++++++++++-------- .../humanoid_pose_finder/planner.py | 25 ++++++++++++------ 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index accb3e32..e5016f1d 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -114,31 +114,33 @@ number_of_joints=number_of_joints, ) - references.com = np.array([0.0, 0.0, 0.7]) + references.state.com = np.array([0.0, 0.0, 0.7]) desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() ) desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() ) - references.contact_points.left = hp_rp.FootContactState.from_parent_frame_transform( - descriptor=planner_settings.contact_points.left, - transform=desired_left_foot_pose, + references.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.left, + transform=desired_left_foot_pose, + ) ) - references.contact_points.right = ( + references.state.contact_points.right = ( hp_rp.FootContactState.from_parent_frame_transform( descriptor=planner_settings.contact_points.right, transform=desired_right_foot_pose, ) ) - references.kinematics.base.quaternion_xyzw = ( + references.state.kinematics.base.quaternion_xyzw = ( liecasadi.SO3.Identity().as_quat().coeffs() ) references.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() - references.kinematics.joints.positions = np.deg2rad( + references.state.kinematics.joints.positions = np.deg2rad( [ 7, 0.12, @@ -190,11 +192,13 @@ desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() ) - references.contact_points.left = hp_rp.FootContactState.from_parent_frame_transform( - descriptor=planner_settings.contact_points.left, - transform=desired_left_foot_pose, + references.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=planner_settings.contact_points.left, + transform=desired_left_foot_pose, + ) ) - references.contact_points.right = ( + references.state.contact_points.right = ( hp_rp.FootContactState.from_parent_frame_transform( descriptor=planner_settings.contact_points.right, transform=desired_right_foot_pose, diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 77f61478..eac03566 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -121,16 +121,25 @@ def is_valid(self) -> bool: @dataclasses.dataclass -class References(hp_rp.HumanoidState): +class References(hp.OptimizationObject): + state: hp_rp.HumanoidState = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState + ) frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + contact_point_descriptors: dataclasses.InitVar[ + hp_rp.FeetContactPointDescriptors + ] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + def __post_init__( self, contact_point_descriptors: hp_rp.FeetContactPointDescriptors, number_of_joints: int, ): - hp_rp.HumanoidState.__post_init__( - self, contact_point_descriptors, number_of_joints + self.state = hp_rp.HumanoidState( + contact_point_descriptors=contact_point_descriptors, + number_of_joints=number_of_joints, ) self.frame_quaternion_xyzw = np.zeros(4) self.frame_quaternion_xyzw[3] = 1.0 @@ -267,11 +276,11 @@ def __init__(self, settings: Settings): self.add_foot_regularization( points=variables.state.contact_points.left, - references=variables.references.contact_points.left, + references=variables.references.state.contact_points.left, ) self.add_foot_regularization( points=variables.state.contact_points.right, - references=variables.references.contact_points.right, + references=variables.references.state.contact_points.right, ) def get_function_inputs_dict(self): @@ -382,7 +391,7 @@ def add_kinematics_regularization( quaternion_error_fun = hp_rp.quaternion_xyzw_error(**function_inputs) quaternion_error = quaternion_error_fun( q=variables.state.kinematics.base.quaternion_xyzw, - qd=variables.references.kinematics.base.quaternion_xyzw, + qd=variables.references.state.kinematics.base.quaternion_xyzw, )["quaternion_error"] problem.add_cost( expression=cs.sumsqr(quaternion_error), @@ -409,7 +418,7 @@ def add_kinematics_regularization( ) # Desired center of mass position - com_position_error = variables.state.com - variables.references.com + com_position_error = variables.state.com - variables.references.state.com problem.add_cost( expression=cs.sumsqr(com_position_error), name="com_position_error", @@ -419,7 +428,7 @@ def add_kinematics_regularization( # Desired joint positions joint_positions_error = ( variables.state.kinematics.joints.positions - - variables.references.kinematics.joints.positions + - variables.references.state.kinematics.joints.positions ) weighted_error = ( From cff53ebb0703c78786066386f66ffa986306fd68 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 15:46:13 +0200 Subject: [PATCH 050/100] Fixed type hinting in pose finder --- src/hippopt/turnkey_planners/humanoid_pose_finder/main.py | 2 +- src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index e5016f1d..65987a4d 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -208,7 +208,7 @@ planner.set_references(references) output = planner.solve() - visualizer.visualize(output.values.state) # noqa + visualizer.visualize(output.values.state) print("Press [Enter] to close.") input() diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index eac03566..1c67223c 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -571,5 +571,5 @@ def set_references(self, references: References) -> None: guess.references = references self.set_initial_guess(guess) - def solve(self) -> hp.Output[hp_rp.HumanoidState]: + def solve(self) -> hp.Output[Variables]: return self.op.problem.solve() From 99fa7dfb04bc9768a0a9db3624361578ac0e0b26 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 16:06:27 +0200 Subject: [PATCH 051/100] Fixed typo in message in PoseFinder --- src/hippopt/turnkey_planners/humanoid_pose_finder/main.py | 2 +- 1 file changed, 1 insertion(+), 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 65987a4d..23ba4c95 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -182,7 +182,7 @@ visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) visualizer.visualize(output.values.state) # noqa - print("Press [Enter] to move to the compute the next pose.") + print("Press [Enter] to move to the next pose.") input() references.com = np.array([0.15, 0.0, 0.7]) From 427e1888f685e8933a74a3c0f4df6e8d26f7fce0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 18:10:03 +0200 Subject: [PATCH 052/100] Setting the initial condition of the flat ground planner from the pose finder --- .../humanoid_walking_flat_ground/.gitignore | 2 + .../humanoid_walking_flat_ground/main.py | 271 +++++++++++++----- .../humanoid_walking_flat_ground/planner.py | 15 +- 3 files changed, 212 insertions(+), 76 deletions(-) create mode 100644 src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore new file mode 100644 index 00000000..f48bb6bf --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore @@ -0,0 +1,2 @@ +*.stl +*.urdf diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index ff322470..ec761f60 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -2,26 +2,27 @@ import casadi as cs import idyntree.bindings as idyntree +import liecasadi import numpy as np import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder import hippopt.turnkey_planners.humanoid_walking_flat_ground.planner as walking_planner -if __name__ == "__main__": + +def get_planner_settings() -> walking_planner.Settings: parser = argparse.ArgumentParser( description="Trajectory Optimization of a forward walking motion on ergoCub.", ) - parser.add_argument( "--urdf", type=str, required=True, help="Path to the ergoCubGazeboV1_minContacts URDF file.", ) - - planner_settings = walking_planner.Settings() - planner_settings.robot_urdf = parser.parse_args().urdf - planner_settings.joints_name_list = [ + settings = walking_planner.Settings() + settings.robot_urdf = parser.parse_args().urdf + settings.joints_name_list = [ "torso_pitch", "torso_roll", "torso_yaw", @@ -46,79 +47,213 @@ "r_ankle_pitch", "r_ankle_roll", ] - number_of_joints = len(planner_settings.joints_name_list) - + number_of_joints = len(settings.joints_name_list) idyntree_model_loader = idyntree.ModelLoader() idyntree_model_loader.loadReducedModelFromFile( - planner_settings.robot_urdf, planner_settings.joints_name_list + settings.robot_urdf, settings.joints_name_list ) idyntree_model = idyntree_model_loader.model() + settings.root_link = "root_link" + settings.horizon_length = 20 + 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 = 20.0 + settings.dcc_epsilon = 0.05 + settings.static_friction = 0.3 + settings.maximum_velocity_control = [2.0, 2.0, 5.0] + settings.maximum_force_derivative = [100.0, 100.0, 100.0] + settings.maximum_angular_momentum = 10.0 + settings.minimum_com_height = 0.3 + settings.minimum_feet_lateral_distance = 0.1 + settings.maximum_feet_relative_height = 0.05 + settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) + settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) + for i in range(number_of_joints): + joint = idyntree_model.getJoint(i) + if joint.hasPosLimits(): + settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) + settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) + settings.maximum_joint_velocities = np.ones(number_of_joints) * 2.0 + settings.minimum_joint_velocities = np.ones(number_of_joints) * -2.0 + settings.joint_regularization_cost_weights = np.ones(number_of_joints) + settings.joint_regularization_cost_weights[:3] = 0.1 # torso + settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + settings.joint_regularization_cost_weights[11:] = 1.0 # legs + settings.contacts_centroid_cost_multiplier = 100.0 + settings.com_linear_velocity_cost_weights = [10.0, 0.1, 1.0] + settings.com_linear_velocity_cost_multiplier = 1.0 + settings.desired_frame_quaternion_cost_frame_name = "chest" + settings.desired_frame_quaternion_cost_multiplier = 90.0 + settings.base_quaternion_cost_multiplier = 50.0 + settings.base_quaternion_velocity_cost_multiplier = 0.001 + settings.joint_regularization_cost_multiplier = 0.1 + settings.force_regularization_cost_multiplier = 0.2 + settings.foot_yaw_regularization_cost_multiplier = 2000.0 + settings.swing_foot_height_cost_multiplier = 1000.0 + settings.contact_velocity_control_cost_multiplier = 5.0 + settings.contact_force_control_cost_multiplier = 0.0001 + settings.casadi_function_options = {} + settings.casadi_opti_options = {} + settings.casadi_solver_options = {} + + return settings + + +def get_pose_finder_settings( + input_settings: walking_planner.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 = input_settings.casadi_solver_options + + return settings + + +def get_visualizer_settings( + input_settings: walking_planner.Settings, +) -> hp_rp.HumanoidStateVisualizerSettings: + visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() + visualizer_settings.robot_model = input_settings.robot_urdf + visualizer_settings.considered_joints = input_settings.joints_name_list + visualizer_settings.contact_points = input_settings.contact_points + visualizer_settings.terrain = input_settings.terrain + visualizer_settings.working_folder = "./" - planner_settings.root_link = "root_link" - planner_settings.horizon_length = 20 - planner_settings.time_step = 0.1 - - planner_settings.contact_points = hp_rp.FeetContactPointDescriptors() - planner_settings.contact_points.left = ( - hp_rp.ContactPointDescriptor.rectangular_foot( - foot_frame="l_sole", - x_length=0.232, - y_length=0.1, - top_left_point_position=np.array([0.116, 0.05, 0.0]), + return visualizer_settings + + +def compute_initial_state( + input_settings: walking_planner.Settings, + pf_input: pose_finder.Planner, +) -> hp_rp.HumanoidState: + desired_joints = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + assert len(input_settings.joints_name_list) == len(desired_joints) + + pf_ref = pose_finder.References( + contact_point_descriptors=pf_settings.contact_points, + number_of_joints=len(desired_joints), + ) + + pf_ref.state.com = np.array([0.0, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + pf_ref.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.left, + transform=desired_left_foot_pose, ) ) - planner_settings.contact_points.right = ( - hp_rp.ContactPointDescriptor.rectangular_foot( - foot_frame="r_sole", - x_length=0.232, - y_length=0.1, - top_left_point_position=np.array([0.116, 0.05, 0.0]), + pf_ref.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.right, + transform=desired_right_foot_pose, ) ) - planner_settings.planar_dcc_height_multiplier = 10.0 - planner_settings.dcc_gain = 20.0 - planner_settings.dcc_epsilon = 0.05 - planner_settings.static_friction = 0.3 - planner_settings.maximum_velocity_control = [2.0, 2.0, 5.0] - planner_settings.maximum_force_derivative = [100.0, 100.0, 100.0] - planner_settings.maximum_angular_momentum = 10.0 - planner_settings.minimum_com_height = 0.3 - planner_settings.minimum_feet_lateral_distance = 0.1 - planner_settings.maximum_feet_relative_height = 0.05 + pf_ref.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) - planner_settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) - planner_settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) + pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() - for i in range(number_of_joints): - joint = idyntree_model.getJoint(i) - if joint.hasPosLimits(): - planner_settings.maximum_joint_positions[i] = joint.getMaxPosLimit(i) - planner_settings.minimum_joint_positions[i] = joint.getMinPosLimit(i) - - planner_settings.maximum_joint_velocities = np.ones(number_of_joints) * 2.0 - planner_settings.minimum_joint_velocities = np.ones(number_of_joints) * -2.0 - - planner_settings.joint_regularization_cost_weights = np.ones(number_of_joints) - planner_settings.joint_regularization_cost_weights[:3] = 0.1 # torso - planner_settings.joint_regularization_cost_weights[3:11] = 10.0 # arms - planner_settings.joint_regularization_cost_weights[11:] = 1.0 # legs - - planner_settings.contacts_centroid_cost_multiplier = 100.0 - planner_settings.com_linear_velocity_cost_weights = [10.0, 0.1, 1.0] - planner_settings.com_linear_velocity_cost_multiplier = 1.0 - planner_settings.desired_frame_quaternion_cost_frame_name = "chest" - planner_settings.desired_frame_quaternion_cost_multiplier = 90.0 - planner_settings.base_quaternion_cost_multiplier = 50.0 - planner_settings.base_quaternion_velocity_cost_multiplier = 0.001 - planner_settings.joint_regularization_cost_multiplier = 0.1 - planner_settings.force_regularization_cost_multiplier = 0.2 - planner_settings.foot_yaw_regularization_cost_multiplier = 2000.0 - planner_settings.swing_foot_height_cost_multiplier = 1000.0 - planner_settings.contact_velocity_control_cost_multiplier = 5.0 - planner_settings.contact_force_control_cost_multiplier = 0.0001 - planner_settings.casadi_function_options = {} - planner_settings.casadi_opti_options = {} - planner_settings.casadi_solver_options = {} + pf_ref.state.kinematics.joints.positions = desired_joints + pf_input.set_references(pf_ref) + + output_pf = pf_input.solve() + + return output_pf.values.state + + +if __name__ == "__main__": + planner_settings = get_planner_settings() planner = walking_planner.Planner(settings=planner_settings) + + pf_settings = get_pose_finder_settings(input_settings=planner_settings) + pf = pose_finder.Planner(settings=pf_settings) + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + + initial_state = compute_initial_state( + input_settings=planner_settings, + pf_input=pf, + ) + visualizer.visualize(initial_state) + + planner.set_initial_state(initial_state) + + planner.solve() diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 1803d249..55d71bf3 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -1117,14 +1117,13 @@ def set_references(self, references: References | list[References]) -> None: guess = self.get_initial_guess() assert isinstance(guess.references, list) - if isinstance(references, list): - assert len(references) == len(guess.references) - for i in range(len(guess.references)): - guess.references[i] = references[i] - else: - for i in range(len(guess.references)): - guess.references[i] = references - self.set_initial_guess(guess) + assert not isinstance(references, list) or len(references) == len( + guess.references + ) + for i in range(len(guess.references)): + guess.references[i] = ( + references[i] if isinstance(references, list) else references + ) def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None: guess = self.get_initial_guess() From 02f6c77eeffbe706e2c2a71075a62eb036357dec Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 19:27:38 +0200 Subject: [PATCH 053/100] Reformat of flat ground planner to ease the conversion of the output to HumanoidState This is for visualization purposes --- .../humanoid_walking_flat_ground/planner.py | 204 +++++++++++------- 1 file changed, 123 insertions(+), 81 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 55d71bf3..42ee8d0e 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -11,25 +11,6 @@ import hippopt.robot_planning as hp_rp -@dataclasses.dataclass -class ExtendedContactPoint( - hp_rp.ContactPointState, - hp_rp.ContactPointStateDerivative, -): - u_v: hp.StorageType = hp.default_storage_field(hp.Variable) - - def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: - hp_rp.ContactPointState.__post_init__(self, input_descriptor) - hp_rp.ContactPointStateDerivative.__post_init__(self) - self.u_v = np.zeros(3) - - -@dataclasses.dataclass -class FeetContactPointsExtended(hp.OptimizationObject): - left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) - right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) - - @dataclasses.dataclass class Settings: robot_urdf: str = dataclasses.field(default=None) @@ -249,18 +230,78 @@ def __post_init__( @dataclasses.dataclass -class Variables(hp.OptimizationObject): +class ExtendedContactPoint( + hp_rp.ContactPointState, + hp_rp.ContactPointStateDerivative, +): + u_v: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: + hp_rp.ContactPointState.__post_init__(self, input_descriptor) + hp_rp.ContactPointStateDerivative.__post_init__(self) + self.u_v = np.zeros(3) + + +@dataclasses.dataclass +class FeetContactPointsExtended(hp.OptimizationObject): + left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + + +@dataclasses.dataclass +class ExtendedHumanoid(hp.OptimizationObject): contact_points: hp.CompositeType[ FeetContactPointsExtended ] = hp.default_composite_field(factory=FeetContactPointsExtended) - com: hp.StorageType = hp.default_storage_field(hp.Variable) - centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) - mass: hp.StorageType = hp.default_storage_field(hp.Parameter) kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( cls=hp.Variable, factory=hp_rp.FloatingBaseSystem ) + com: hp.StorageType = hp.default_storage_field(hp.Variable) + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + + contact_point_descriptors: dataclasses.InitVar[ + hp_rp.FeetContactPointDescriptors + ] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + if contact_point_descriptors is not None: + self.contact_points.left = [ + ExtendedContactPoint(input_descriptor=point) + for point in contact_point_descriptors.left + ] + self.contact_points.right = [ + ExtendedContactPoint(input_descriptor=point) + for point in contact_point_descriptors.right + ] + + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(6) + self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) + + def to_humanoid_state(self): + return hp_rp.HumanoidState( + contact_points=self.contact_points, + kinematics=self.kinematics, + com=self.com, + centroidal_momentum=self.centroidal_momentum, + ) + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + system: hp.CompositeType[ExtendedHumanoid] = hp.default_composite_field( + cls=hp.Variable, factory=ExtendedHumanoid + ) + + mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + initial_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False ) @@ -302,18 +343,10 @@ def __post_init__( settings: Settings, kin_dyn_object: adam.casadi.KinDynComputations, ) -> None: - self.contact_points.left = [ - ExtendedContactPoint(input_descriptor=point) - for point in settings.contact_points.left - ] - self.contact_points.right = [ - ExtendedContactPoint(input_descriptor=point) - for point in settings.contact_points.right - ] - - self.com = np.zeros(3) - self.centroidal_momentum = np.zeros(6) - self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=kin_dyn_object.NDoF) + self.system = ExtendedHumanoid( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) self.initial_state = hp_rp.HumanoidState( contact_point_descriptors=settings.contact_points, @@ -385,7 +418,7 @@ def __init__(self, settings: Settings) -> None: **function_inputs ) normalized_quaternion = normalized_quaternion_fun( - q=sym.kinematics.base.quaternion_xyzw + q=sym.system.kinematics.base.quaternion_xyzw )["quaternion_normalized"] # Align names used in the terrain function with those in function_inputs @@ -409,7 +442,9 @@ def __init__(self, settings: Settings) -> None: ) point_kinematics_functions = {} - all_contact_points = sym.contact_points.left + sym.contact_points.right + all_contact_points = ( + sym.system.contact_points.left + sym.system.contact_points.right + ) all_contact_initial_state = ( sym.initial_state.contact_points.left + sym.initial_state.contact_points.right @@ -453,14 +488,14 @@ def __init__(self, settings: Settings) -> None: self.add_contact_centroids_expressions(function_inputs) self.add_foot_regularization( - points=sym.contact_points.left, + points=sym.system.contact_points.left, descriptors=self.settings.contact_points.left, references=sym.references.feet.left, function_inputs=function_inputs, foot_name="left", ) self.add_foot_regularization( - points=sym.contact_points.right, + points=sym.system.contact_points.right, descriptors=self.settings.contact_points.right, references=sym.references.feet.right, function_inputs=function_inputs, @@ -471,8 +506,8 @@ def get_function_inputs_dict(self): sym = self.ocp.symbolic_structure function_inputs = { "mass_name": sym.mass.name(), - "momentum_name": sym.centroidal_momentum.name(), - "com_name": sym.com.name(), + "momentum_name": sym.system.centroidal_momentum.name(), + "com_name": sym.system.com.name(), "quaternion_xyzw_name": "q", "gravity_name": sym.gravity.name(), "point_position_names": [], @@ -504,7 +539,7 @@ def get_function_inputs_dict(self): def add_contact_centroids_expressions(self, function_inputs): problem = self.ocp.problem - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables # Maximum feet relative height def get_centroid( @@ -521,10 +556,10 @@ def get_centroid( return centroid_fun(**point_position_dict)["centroid"] left_centroid = get_centroid( - points=sym.contact_points.left, function_inputs_dict=function_inputs + points=sym.system.contact_points.left, function_inputs_dict=function_inputs ) right_centroid = get_centroid( - points=sym.contact_points.right, function_inputs_dict=function_inputs + points=sym.system.contact_points.right, function_inputs_dict=function_inputs ) problem.add_expression_to_horizon( expression=cs.Opti_bounded( @@ -560,11 +595,13 @@ def add_kinematics_constraints( normalized_quaternion: cs.MX, ): problem = self.ocp.problem - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables # Unitary quaternion problem.add_expression_to_horizon( - expression=cs.MX(cs.sumsqr(sym.kinematics.base.quaternion_xyzw) == 1), + expression=cs.MX( + cs.sumsqr(sym.system.kinematics.base.quaternion_xyzw) == 1 + ), apply_to_first_elements=False, name="unitary_quaternion", ) @@ -574,12 +611,12 @@ def add_kinematics_constraints( kindyn_object=self.kin_dyn_object, **function_inputs ) com_kinematics = com_kinematics_fun( - pb=sym.kinematics.base.position, + pb=sym.system.kinematics.base.position, qb=normalized_quaternion, - s=sym.kinematics.joints.positions, + s=sym.system.kinematics.joints.positions, )["com_position"] problem.add_expression_to_horizon( - expression=cs.MX(sym.com == com_kinematics), + expression=cs.MX(sym.system.com == com_kinematics), apply_to_first_elements=False, name="com_kinematics_consistency", ) @@ -589,15 +626,17 @@ def add_kinematics_constraints( kindyn_object=self.kin_dyn_object, **function_inputs ) centroidal_kinematics = centroidal_kinematics_fun( - pb=sym.kinematics.base.position, + pb=sym.system.kinematics.base.position, qb=normalized_quaternion, - s=sym.kinematics.joints.positions, - pb_dot=sym.kinematics.base.linear_velocity, - qb_dot=sym.kinematics.base.quaternion_velocity_xyzw, - s_dot=sym.kinematics.joints.velocities, + 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.centroidal_momentum[3:] == centroidal_kinematics[3:]), + expression=cs.MX( + sym.system.centroidal_momentum[3:] == centroidal_kinematics[3:] + ), apply_to_first_elements=True, name="centroidal_momentum_kinematics_consistency", ) @@ -606,7 +645,7 @@ def add_kinematics_constraints( problem.add_expression_to_horizon( expression=cs.Opti_bounded( -sym.maximum_angular_momentum, - sym.centroidal_momentum[3:], + sym.system.centroidal_momentum[3:], sym.maximum_angular_momentum, ), apply_to_first_elements=True, @@ -614,7 +653,7 @@ def add_kinematics_constraints( ) # Minimum com height - com_height = height_fun(p=sym.com)["point_height"] + com_height = height_fun(p=sym.system.com)["point_height"] problem.add_expression_to_horizon( expression=cs.MX(com_height >= sym.minimum_com_height), apply_to_first_elements=False, @@ -622,17 +661,17 @@ def add_kinematics_constraints( ) # Minimum feet lateral distance - left_frame = sym.contact_points.left[0].descriptor.foot_frame - right_frame = sym.contact_points.right[0].descriptor.foot_frame + left_frame = sym.system.contact_points.left[0].descriptor.foot_frame + right_frame = sym.system.contact_points.right[0].descriptor.foot_frame relative_position_fun = hp_rp.frames_relative_position( kindyn_object=self.kin_dyn_object, reference_frame=right_frame, target_frame=left_frame, **function_inputs, ) - relative_position = relative_position_fun(s=sym.kinematics.joints.positions)[ - "relative_position" - ] + relative_position = relative_position_fun( + s=sym.system.kinematics.joints.positions + )["relative_position"] problem.add_expression_to_horizon( expression=cs.MX( relative_position[:2] >= sym.minimum_feet_lateral_distance @@ -645,7 +684,7 @@ def add_kinematics_constraints( problem.add_expression_to_horizon( expression=cs.Opti_bounded( sym.minimum_joint_positions, - sym.kinematics.joints.positions, + sym.system.kinematics.joints.positions, sym.maximum_joint_positions, ), apply_to_first_elements=False, @@ -656,7 +695,7 @@ def add_kinematics_constraints( problem.add_expression_to_horizon( expression=cs.Opti_bounded( sym.minimum_joint_velocities, - sym.kinematics.joints.velocities, + sym.system.kinematics.joints.velocities, sym.maximum_joint_velocities, ), apply_to_first_elements=True, @@ -667,10 +706,11 @@ def add_kinematics_regularization( self, function_inputs: dict, base_quaternion_normalized: cs.MX ): problem = self.ocp.problem - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables # Desired com velocity com_velocity_error = ( - sym.centroidal_momentum[:3] - sym.references.com_linear_velocity * sym.mass + sym.system.centroidal_momentum[:3] + - sym.references.com_linear_velocity * sym.mass ) com_velocity_weighted_error = ( com_velocity_error.T @@ -692,9 +732,9 @@ def add_kinematics_regularization( **function_inputs, ) rotation_error_kinematics = rotation_error_kinematics_fun( - pb=sym.kinematics.base.position, + pb=sym.system.kinematics.base.position, qb=base_quaternion_normalized, - s=sym.kinematics.joints.positions, + s=sym.system.kinematics.joints.positions, qd=sym.references.desired_frame_quaternion_xyzw, )["rotation_error"] problem.add_expression_to_horizon( @@ -708,7 +748,7 @@ def add_kinematics_regularization( # Desired base orientation quaternion_error_fun = hp_rp.quaternion_xyzw_error(**function_inputs) quaternion_error = quaternion_error_fun( - q=sym.kinematics.base.quaternion_xyzw, + q=sym.system.kinematics.base.quaternion_xyzw, qd=sym.references.base_quaternion_xyzw, )["quaternion_error"] problem.add_expression_to_horizon( @@ -722,7 +762,7 @@ def add_kinematics_regularization( # Desired base angular velocity problem.add_expression_to_horizon( expression=cs.sumsqr( - sym.kinematics.base.quaternion_velocity_xyzw + sym.system.kinematics.base.quaternion_velocity_xyzw - sym.references.base_quaternion_xyzw_velocity ), apply_to_first_elements=True, @@ -733,10 +773,10 @@ def add_kinematics_regularization( # Desired joint positions joint_positions_error = ( - sym.kinematics.joints.positions - sym.references.joint_regularization + sym.system.kinematics.joints.positions - sym.references.joint_regularization ) joint_velocity_error = ( - sym.kinematics.joints.velocities + sym.system.kinematics.joints.velocities + cs.diag(cs.DM(self.settings.joint_regularization_cost_weights)) * joint_positions_error ) @@ -755,7 +795,8 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(pb) = pb_dot (base position dynamics) problem.add_dynamics( - hp.dot(sym.kinematics.base.position) == sym.kinematics.base.linear_velocity, + hp.dot(sym.system.kinematics.base.position) + == sym.system.kinematics.base.linear_velocity, x0=problem.initial(sym.initial_state.kinematics.base.position), dt=sym.dt, integrator=default_integrator, @@ -764,8 +805,8 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(q) = q_dot (base quaternion dynamics) problem.add_dynamics( - hp.dot(sym.kinematics.base.quaternion_xyzw) - == sym.kinematics.base.quaternion_velocity_xyzw, + hp.dot(sym.system.kinematics.base.quaternion_xyzw) + == sym.system.kinematics.base.quaternion_velocity_xyzw, x0=problem.initial(sym.initial_state.kinematics.base.quaternion_xyzw), dt=sym.dt, integrator=default_integrator, @@ -774,7 +815,8 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(s) = s_dot (joint position dynamics) problem.add_dynamics( - hp.dot(sym.kinematics.joints.positions) == sym.kinematics.joints.velocities, + hp.dot(sym.system.kinematics.joints.positions) + == sym.system.kinematics.joints.velocities, x0=problem.initial(sym.initial_state.kinematics.joints.positions), dt=sym.dt, integrator=default_integrator, @@ -784,7 +826,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): # dot(com) = h_g[:3]/m (center of mass dynamics) com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) problem.add_dynamics( - hp.dot(sym.com) == com_dynamics, # noqa + hp.dot(sym.system.com) == com_dynamics, # noqa x0=problem.initial(sym.initial_state.com), # noqa dt=sym.dt, integrator=default_integrator, @@ -803,7 +845,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): **function_inputs, ) problem.add_dynamics( - hp.dot(sym.centroidal_momentum) == centroidal_dynamics, # noqa + hp.dot(sym.system.centroidal_momentum) == centroidal_dynamics, # noqa x0=problem.initial(sym.initial_state.centroidal_momentum), # noqa dt=sym.dt, integrator=default_integrator, @@ -818,7 +860,7 @@ def add_contact_kinematic_consistency( point_kinematics_functions: dict, ): problem = self.ocp.problem - sym = self.ocp.symbolic_structure + sym = self.ocp.symbolic_structure # type: Variables # Creation of contact kinematics consistency functions descriptor = point.descriptor @@ -833,9 +875,9 @@ def add_contact_kinematic_consistency( # Consistency between the contact position and the kinematics point_kinematics = point_kinematics_functions[descriptor.foot_frame]( - pb=sym.kinematics.base.position, + pb=sym.system.kinematics.base.position, qb=normalized_quaternion, - s=sym.kinematics.joints.positions, + s=sym.system.kinematics.joints.positions, p_parent=descriptor.position_in_foot_frame, )["point_position"] problem.add_expression_to_horizon( From d7d12aecd7b399bd94d5708be32121d6d89e31bf Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 19:27:54 +0200 Subject: [PATCH 054/100] Added draft to set the flat ground planner references. --- .../humanoid_walking_flat_ground/main.py | 119 ++++++++++++++++-- 1 file changed, 112 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index ec761f60..25008c97 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -155,14 +155,14 @@ def get_pose_finder_settings( def get_visualizer_settings( input_settings: walking_planner.Settings, ) -> hp_rp.HumanoidStateVisualizerSettings: - visualizer_settings = hp_rp.HumanoidStateVisualizerSettings() - visualizer_settings.robot_model = input_settings.robot_urdf - visualizer_settings.considered_joints = input_settings.joints_name_list - visualizer_settings.contact_points = input_settings.contact_points - visualizer_settings.terrain = input_settings.terrain - visualizer_settings.working_folder = "./" + 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 visualizer_settings + return output_viz_settings def compute_initial_state( @@ -238,6 +238,94 @@ def compute_initial_state( return output_pf.values.state +def compute_final_state( + input_settings: walking_planner.Settings, + pf_input: pose_finder.Planner, +) -> hp_rp.HumanoidState: + desired_joints = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + assert len(input_settings.joints_name_list) == len(desired_joints) + + pf_ref = pose_finder.References( + contact_point_descriptors=pf_settings.contact_points, + number_of_joints=len(desired_joints), + ) + + pf_ref.state.com = np.array([0.15, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + pf_ref.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + pf_ref.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + pf_ref.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + + pf_ref.state.kinematics.joints.positions = desired_joints + + pf_input.set_references(pf_ref) + + output_pf = pf_input.solve() + return output_pf.values.state + + +def compute_references( + input_settings: walking_planner.Settings, desired_state: hp_rp.HumanoidState +) -> walking_planner.References: + output_reference = walking_planner.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.15, 0.0, 0.0] + output_reference.joint_regularization = desired_state.kinematics.joints.positions + + return output_reference + + if __name__ == "__main__": planner_settings = get_planner_settings() planner = walking_planner.Planner(settings=planner_settings) @@ -256,4 +344,21 @@ def compute_initial_state( planner.set_initial_state(initial_state) + final_state = compute_final_state( + input_settings=planner_settings, + pf_input=pf, + ) + + print("Press [Enter] to visualize the final desired state.") + input() + + visualizer.visualize(final_state) + + references = compute_references( + input_settings=planner_settings, + desired_state=final_state, + ) + + planner.set_references(references) + planner.solve() From 4752d87f6161cafdf305a608172f18af63b3b3de Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 13:03:36 +0200 Subject: [PATCH 055/100] Fixed conversion from ExtendedHumanoid to HumanoidState --- .../humanoid_walking_flat_ground/planner.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 42ee8d0e..e039de58 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -286,12 +286,13 @@ def __post_init__( self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) def to_humanoid_state(self): - return hp_rp.HumanoidState( - contact_points=self.contact_points, - kinematics=self.kinematics, - com=self.com, - centroidal_momentum=self.centroidal_momentum, - ) + output = hp_rp.HumanoidState() + output.kinematics.base = self.kinematics.base + output.kinematics.joints = self.kinematics.joints + output.contact_points = self.contact_points + output.com = self.com + output.centroidal_momentum = self.centroidal_momentum + return output @dataclasses.dataclass From 29563f29cbb0089c091dbcdae9b17ddbb5f3ff00 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 13:14:26 +0200 Subject: [PATCH 056/100] Fixed bug in feet lateral position --- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index e039de58..8fe2204b 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -674,9 +674,7 @@ def add_kinematics_constraints( s=sym.system.kinematics.joints.positions )["relative_position"] problem.add_expression_to_horizon( - expression=cs.MX( - relative_position[:2] >= sym.minimum_feet_lateral_distance - ), + expression=cs.MX(relative_position[1] >= sym.minimum_feet_lateral_distance), apply_to_first_elements=False, name="minimum_feet_distance", ) From 76b8f9fee7c9530b9890b95f12d1dcfa4f36544a Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 14:14:55 +0200 Subject: [PATCH 057/100] First standing result --- .../humanoid_walking_flat_ground/main.py | 54 +++++++++++++++---- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index 25008c97..e7d61570 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -105,9 +105,32 @@ def get_planner_settings() -> walking_planner.Settings: settings.swing_foot_height_cost_multiplier = 1000.0 settings.contact_velocity_control_cost_multiplier = 5.0 settings.contact_force_control_cost_multiplier = 0.0001 - settings.casadi_function_options = {} - settings.casadi_opti_options = {} - settings.casadi_solver_options = {} + settings.casadi_function_options = {"cse": True} + settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True} + settings.casadi_solver_options = { + "max_iter": 4000, + "linear_solver": "mumps", + "alpha_for_y": "dual-and-full", + "fast_step_computation": "yes", + "hessian_approximation": "limited-memory", + "tol": 1e-3, + "dual_inf_tol": 1000.0, + "compl_inf_tol": 1e-2, + "constr_viol_tol": 1e-4, + "acceptable_tol": 1e0, + "acceptable_iter": 2, + "acceptable_compl_inf_tol": 1.0, + "warm_start_bound_frac": 1e-2, + "warm_start_bound_push": 1e-2, + "warm_start_mult_bound_push": 1e-2, + "warm_start_slack_bound_frac": 1e-2, + "warm_start_slack_bound_push": 1e-2, + "warm_start_init_point": "yes", + "required_infeasibility_reduction": 0.8, + "perturb_dec_fact": 0.1, + "max_hessian_perturbation": 100.0, + "acceptable_obj_change_tol": 1e0, + } return settings @@ -147,7 +170,7 @@ def get_pose_finder_settings( 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 = input_settings.casadi_solver_options + settings.casadi_solver_options = {} return settings @@ -276,12 +299,12 @@ def compute_final_state( number_of_joints=len(desired_joints), ) - pf_ref.state.com = np.array([0.15, 0.0, 0.7]) + pf_ref.state.com = np.array([0.0, 0.0, 0.7]) desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() ) desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() ) pf_ref.state.contact_points.left = ( hp_rp.FootContactState.from_parent_frame_transform( @@ -310,7 +333,7 @@ def compute_final_state( return output_pf.values.state -def compute_references( +def get_references( input_settings: walking_planner.Settings, desired_state: hp_rp.HumanoidState ) -> walking_planner.References: output_reference = walking_planner.References( @@ -320,7 +343,7 @@ def compute_references( ) output_reference.contacts_centroid_cost_weights = [100, 100, 10] - output_reference.contacts_centroid = [0.15, 0.0, 0.0] + output_reference.contacts_centroid = [0.0, 0.0, 0.0] output_reference.joint_regularization = desired_state.kinematics.joints.positions return output_reference @@ -354,11 +377,22 @@ def compute_references( visualizer.visualize(final_state) - references = compute_references( + references = get_references( input_settings=planner_settings, desired_state=final_state, ) planner.set_references(references) - planner.solve() + output = planner.solve() + + humanoid_states = [s.to_humanoid_state() for s in output.values.system] + + print("Press [Enter] to visualize the solution.") + input() + + visualizer.visualize( + state=humanoid_states, timestep_s=output.values.dt, time_multiplier=10.0 + ) + + # TODO: Move to mass normalization From 416db207f0ea1a32baad9a6e0ba2dd96f0673474 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 17:58:38 +0200 Subject: [PATCH 058/100] Check that gravity is 6 dimensional in the pose finder --- src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 1c67223c..b43bc159 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -72,7 +72,7 @@ def is_valid(self) -> bool: if self.root_link is None: logger.error("root_link is None") ok = False - if self.gravity is None: + if self.gravity is None or len(self.gravity) != 6: logger.error("gravity is None") ok = False if self.terrain is None: From 3b855bd243690c1ac38e26d2bcbbdea2e59451f2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 17:59:08 +0200 Subject: [PATCH 059/100] Fixed setting of references and guesses in flat ground planner --- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 8fe2204b..44fdbe1b 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -1165,11 +1165,12 @@ def set_references(self, references: References | list[References]) -> None: guess.references[i] = ( references[i] if isinstance(references, list) else references ) + self.ocp.problem.set_initial_guess(guess) def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None: guess = self.get_initial_guess() guess.initial_state = initial_state - self.set_initial_guess(guess) + self.ocp.problem.set_initial_guess(guess) def solve(self) -> hp.Output[Variables]: return self.ocp.problem.solve() From c6949402a6956ba4bb435ca5154827216a458b3a Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 18:53:14 +0200 Subject: [PATCH 060/100] Using the mass regularized version of the dynamics in the planar ground planner --- .../humanoid_walking_flat_ground/planner.py | 25 +++++++++++++------ 1 file changed, 17 insertions(+), 8 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 44fdbe1b..bf914b05 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -285,11 +285,13 @@ def __post_init__( self.centroidal_momentum = np.zeros(6) self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) - def to_humanoid_state(self): + def to_humanoid_state(self, mass: float = 1.0) -> hp_rp.HumanoidState: output = hp_rp.HumanoidState() output.kinematics.base = self.kinematics.base output.kinematics.joints = self.kinematics.joints output.contact_points = self.contact_points + for point in output.contact_points.left + output.contact_points.right: + point.f *= mass output.com = self.com output.centroidal_momentum = self.centroidal_momentum return output @@ -391,6 +393,7 @@ def __init__(self, settings: Settings) -> None: 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 @@ -636,7 +639,8 @@ def add_kinematics_constraints( )["h_g"] problem.add_expression_to_horizon( expression=cs.MX( - sym.system.centroidal_momentum[3:] == centroidal_kinematics[3:] + sym.system.centroidal_momentum[3:] + == centroidal_kinematics[3:] / sym.mass ), apply_to_first_elements=True, name="centroidal_momentum_kinematics_consistency", @@ -708,8 +712,7 @@ def add_kinematics_regularization( sym = self.ocp.symbolic_structure # type: Variables # Desired com velocity com_velocity_error = ( - sym.system.centroidal_momentum[:3] - - sym.references.com_linear_velocity * sym.mass + sym.system.centroidal_momentum[:3] - sym.references.com_linear_velocity ) com_velocity_weighted_error = ( com_velocity_error.T @@ -822,17 +825,16 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): name="joint_position_dynamics", ) - # dot(com) = h_g[:3]/m (center of mass dynamics) - com_dynamics = hp_rp.com_dynamics_from_momentum(**function_inputs) + # dot(com) = h_g[:3] (center of mass dynamics, regularized by the mass) problem.add_dynamics( - hp.dot(sym.system.com) == com_dynamics, # noqa + hp.dot(sym.system.com) == sym.system.centroidal_momentum[:3], # noqa x0=problem.initial(sym.initial_state.com), # noqa dt=sym.dt, integrator=default_integrator, name="com_dynamics", ) - # dot(h) = sum_i (p_i x f_i) + mg (centroidal momentum dynamics) + # dot(h) = sum_i (p_i x f_i) + g (centroidal momentum dynamics,mass regularized) function_inputs["point_position_names"] = [ point.p.name() for point in all_contact_points ] @@ -841,6 +843,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): ] centroidal_dynamics = hp_rp.centroidal_dynamics_with_point_forces( number_of_points=len(function_inputs["point_position_names"]), + assume_unitary_mass=True, **function_inputs, ) problem.add_dynamics( @@ -1170,6 +1173,12 @@ def set_references(self, references: References | list[References]) -> None: def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None: guess = self.get_initial_guess() guess.initial_state = initial_state + guess.initial_state.centroidal_momentum /= self.numeric_mass + for point in ( + guess.initial_state.contact_points.left + + guess.initial_state.contact_points.right + ): + point.f /= self.numeric_mass self.ocp.problem.set_initial_guess(guess) def solve(self) -> hp.Output[Variables]: From daea2fda93be33902bb2778392bab854332aace3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 18:53:32 +0200 Subject: [PATCH 061/100] First imprecise walking --- .../humanoid_walking_flat_ground/main.py | 20 ++++++++++++------- 1 file changed, 13 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index e7d61570..c783e145 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -1,4 +1,5 @@ import argparse +import logging import casadi as cs import idyntree.bindings as idyntree @@ -258,6 +259,8 @@ def compute_initial_state( output_pf = pf_input.solve() + output_pf.values.state.centroidal_momentum = np.zeros((6, 1)) + return output_pf.values.state @@ -299,12 +302,12 @@ def compute_final_state( number_of_joints=len(desired_joints), ) - pf_ref.state.com = np.array([0.0, 0.0, 0.7]) + pf_ref.state.com = np.array([0.15, 0.0, 0.7]) desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() ) desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() ) pf_ref.state.contact_points.left = ( hp_rp.FootContactState.from_parent_frame_transform( @@ -343,13 +346,16 @@ def get_references( ) output_reference.contacts_centroid_cost_weights = [100, 100, 10] - output_reference.contacts_centroid = [0.0, 0.0, 0.0] + output_reference.contacts_centroid = [0.3, 0.0, 0.0] output_reference.joint_regularization = desired_state.kinematics.joints.positions + output_reference.com_linear_velocity = [0.1, 0.0, 0.0] return output_reference if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + planner_settings = get_planner_settings() planner = walking_planner.Planner(settings=planner_settings) @@ -386,13 +392,13 @@ def get_references( output = planner.solve() - humanoid_states = [s.to_humanoid_state() for s in output.values.system] + humanoid_states = [ + s.to_humanoid_state(output.values.mass) for s in output.values.system + ] print("Press [Enter] to visualize the solution.") input() visualizer.visualize( - state=humanoid_states, timestep_s=output.values.dt, time_multiplier=10.0 + state=humanoid_states, timestep_s=output.values.dt, time_multiplier=2.0 ) - - # TODO: Move to mass normalization From f2193db21effbab2f68afa89d6facf5543d3c827 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 18 Oct 2023 15:31:48 +0200 Subject: [PATCH 062/100] Fixed setting of force derivative bounds. Added visualization of final video --- .../humanoid_walking_flat_ground/.gitignore | 3 +++ .../humanoid_walking_flat_ground/main.py | 14 ++++++++++++-- 2 files changed, 15 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore index f48bb6bf..71007872 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore @@ -1,2 +1,5 @@ *.stl *.urdf +frames/* +*.png +*.mp4 diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index c783e145..2ff3c7b0 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -75,8 +75,8 @@ def get_planner_settings() -> walking_planner.Settings: settings.dcc_epsilon = 0.05 settings.static_friction = 0.3 settings.maximum_velocity_control = [2.0, 2.0, 5.0] - settings.maximum_force_derivative = [100.0, 100.0, 100.0] - settings.maximum_angular_momentum = 10.0 + settings.maximum_force_derivative = [1.0, 1.0, 1.0] + settings.maximum_angular_momentum = 1.0 settings.minimum_com_height = 0.3 settings.minimum_feet_lateral_distance = 0.1 settings.maximum_feet_relative_height = 0.05 @@ -383,6 +383,8 @@ def get_references( visualizer.visualize(final_state) + print("Starting the planner...") + references = get_references( input_settings=planner_settings, desired_state=final_state, @@ -402,3 +404,11 @@ def get_references( visualizer.visualize( state=humanoid_states, timestep_s=output.values.dt, time_multiplier=2.0 ) + + visualizer.visualize( + state=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking", + ) From c563d884e286ababf87158d92ae3182d4e529d4c Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 30 Oct 2023 19:22:22 +0100 Subject: [PATCH 063/100] Multiplied the forces times the mass before returning the output solution --- .../humanoid_walking_flat_ground/main.py | 4 +--- .../humanoid_walking_flat_ground/planner.py | 14 ++++++++++---- 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index 2ff3c7b0..e86b39c1 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -394,9 +394,7 @@ def get_references( output = planner.solve() - humanoid_states = [ - s.to_humanoid_state(output.values.mass) for s in output.values.system - ] + humanoid_states = [s.to_humanoid_state() for s in output.values.system] print("Press [Enter] to visualize the solution.") input() diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index bf914b05..8e158e23 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -285,13 +285,11 @@ def __post_init__( self.centroidal_momentum = np.zeros(6) self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) - def to_humanoid_state(self, mass: float = 1.0) -> hp_rp.HumanoidState: + def to_humanoid_state(self) -> hp_rp.HumanoidState: output = hp_rp.HumanoidState() output.kinematics.base = self.kinematics.base output.kinematics.joints = self.kinematics.joints output.contact_points = self.contact_points - for point in output.contact_points.left + output.contact_points.right: - point.f *= mass output.com = self.com output.centroidal_momentum = self.centroidal_momentum return output @@ -1182,4 +1180,12 @@ def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None: self.ocp.problem.set_initial_guess(guess) def solve(self) -> hp.Output[Variables]: - return self.ocp.problem.solve() + output = self.ocp.problem.solve() + + values = output.values + + for s in values.system: + for point in s.contact_points.left + s.contact_points.right: + point.f *= values.mass + + return output From 759a3c0910224b73a04c2a3915ec237fa79fa786 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 31 Oct 2023 13:00:41 +0100 Subject: [PATCH 064/100] Added visualization of complementarity. Exploiting external processes to avoid matplotlib blocking the execution --- .../humanoid_walking_flat_ground/main.py | 20 ++++++++++++++++++- 1 file changed, 19 insertions(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index e86b39c1..ee331c76 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -395,10 +395,28 @@ def get_references( output = planner.solve() humanoid_states = [s.to_humanoid_state() for s in output.values.system] - + left_contact_points = [s.contact_points.left for s in humanoid_states] + right_contact_points = [s.contact_points.right for s in humanoid_states] print("Press [Enter] to visualize the solution.") input() + plotter_settings = hp_rp.FootContactStatePlotterSettings() + plotter_settings.terrain = planner_settings.terrain + left_complementarity_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + left_complementarity_plotter.plot_complementarity( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Complementarity", + blocking=False, + ) + right_complementarity_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + right_complementarity_plotter.plot_complementarity( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Complementarity", + blocking=False, + ) + visualizer.visualize( state=humanoid_states, timestep_s=output.values.dt, time_multiplier=2.0 ) From e95a44d29726b0ed2ea0a695693cc2ffa5004660 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 31 Oct 2023 19:48:17 +0100 Subject: [PATCH 065/100] Added possibility to force close the complementarity plots --- .../turnkey_planners/humanoid_walking_flat_ground/main.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index ee331c76..ef1cd147 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -428,3 +428,8 @@ def get_references( save=True, file_name_stem="humanoid_walking", ) + + print("Press [Enter] to exit.") + input() + left_complementarity_plotter.close() + right_complementarity_plotter.close() From 4048820c5b9d0f1d5492c1fa1ce3cf4619127067 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 2 Nov 2023 19:32:39 +0100 Subject: [PATCH 066/100] Added visualization of forces in flat ground planner --- .../humanoid_walking_flat_ground/main.py | 26 ++++++++++++++----- 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index ef1cd147..0ab1458b 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -402,21 +402,35 @@ def get_references( plotter_settings = hp_rp.FootContactStatePlotterSettings() plotter_settings.terrain = planner_settings.terrain - left_complementarity_plotter = hp_rp.FootContactStatePlotter(plotter_settings) - left_complementarity_plotter.plot_complementarity( + left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + + left_foot_plotter.plot_complementarity( states=left_contact_points, time_s=output.values.dt, title="Left Foot Complementarity", blocking=False, ) - right_complementarity_plotter = hp_rp.FootContactStatePlotter(plotter_settings) - right_complementarity_plotter.plot_complementarity( + right_foot_plotter.plot_complementarity( states=right_contact_points, time_s=output.values.dt, title="Right Foot Complementarity", blocking=False, ) + left_foot_plotter.plot_forces( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Forces", + blocking=False, + ) + right_foot_plotter.plot_forces( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Forces", + blocking=False, + ) + visualizer.visualize( state=humanoid_states, timestep_s=output.values.dt, time_multiplier=2.0 ) @@ -431,5 +445,5 @@ def get_references( print("Press [Enter] to exit.") input() - left_complementarity_plotter.close() - right_complementarity_plotter.close() + left_foot_plotter.close() + right_foot_plotter.close() From fa98209801169864fd9069f70771f26d7361d181 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 17:04:17 +0100 Subject: [PATCH 067/100] Multiply by the mass in the max centroidal momentum and force derivative constraints --- .../turnkey_planners/humanoid_walking_flat_ground/planner.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py index 8e158e23..b564a2fa 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py @@ -648,7 +648,7 @@ def add_kinematics_constraints( problem.add_expression_to_horizon( expression=cs.Opti_bounded( -sym.maximum_angular_momentum, - sym.system.centroidal_momentum[3:], + sym.system.centroidal_momentum[3:] * sym.mass, sym.maximum_angular_momentum, ), apply_to_first_elements=True, @@ -966,7 +966,7 @@ def add_contact_point_feasibility( problem.add_expression_to_horizon( expression=cs.Opti_bounded( -sym.maximum_force_derivative, - point.f_dot, + point.f_dot * sym.mass, sym.maximum_force_derivative, ), apply_to_first_elements=True, From 5d7e69f7c0ed135fcc1afe7486da382ae9b9cfce Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 17:05:33 +0100 Subject: [PATCH 068/100] Tuning for walking forward --- .../humanoid_walking_flat_ground/main.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py index 0ab1458b..45785f32 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py @@ -55,7 +55,7 @@ def get_planner_settings() -> walking_planner.Settings: ) idyntree_model = idyntree_model_loader.model() settings.root_link = "root_link" - settings.horizon_length = 20 + settings.horizon_length = 30 settings.time_step = 0.1 settings.contact_points = hp_rp.FeetContactPointDescriptors() settings.contact_points.left = hp_rp.ContactPointDescriptor.rectangular_foot( @@ -71,12 +71,12 @@ def get_planner_settings() -> walking_planner.Settings: top_left_point_position=np.array([0.116, 0.05, 0.0]), ) settings.planar_dcc_height_multiplier = 10.0 - settings.dcc_gain = 20.0 - settings.dcc_epsilon = 0.05 + 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 = [1.0, 1.0, 1.0] - settings.maximum_angular_momentum = 1.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 @@ -101,7 +101,7 @@ def get_planner_settings() -> walking_planner.Settings: settings.base_quaternion_cost_multiplier = 50.0 settings.base_quaternion_velocity_cost_multiplier = 0.001 settings.joint_regularization_cost_multiplier = 0.1 - settings.force_regularization_cost_multiplier = 0.2 + 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 From 910258bef1a17be7715280c90b74cdc0d61e0bd8 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 18:20:30 +0100 Subject: [PATCH 069/100] Renamed humanoid_walking_flat_ground to humanoid_kinodynamic --- .../.gitignore | 0 .../main.py | 2 +- .../planner.py | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename src/hippopt/turnkey_planners/{humanoid_walking_flat_ground => humanoid_kinodynamic}/.gitignore (100%) rename src/hippopt/turnkey_planners/{humanoid_walking_flat_ground => humanoid_kinodynamic}/main.py (99%) rename src/hippopt/turnkey_planners/{humanoid_walking_flat_ground => humanoid_kinodynamic}/planner.py (100%) diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore b/src/hippopt/turnkey_planners/humanoid_kinodynamic/.gitignore similarity index 100% rename from src/hippopt/turnkey_planners/humanoid_walking_flat_ground/.gitignore rename to src/hippopt/turnkey_planners/humanoid_kinodynamic/.gitignore diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main.py similarity index 99% rename from src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py rename to src/hippopt/turnkey_planners/humanoid_kinodynamic/main.py index 45785f32..39bdc956 100644 --- a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/main.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main.py @@ -7,8 +7,8 @@ import numpy as np import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_kinodynamic.planner as walking_planner import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder -import hippopt.turnkey_planners.humanoid_walking_flat_ground.planner as walking_planner def get_planner_settings() -> walking_planner.Settings: diff --git a/src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py similarity index 100% rename from src/hippopt/turnkey_planners/humanoid_walking_flat_ground/planner.py rename to src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py From ff6ab609b09445f674b03b4ebf2b3e50ed045ff4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 18:29:39 +0100 Subject: [PATCH 070/100] Renamed main to main_single_step_flat_ground.py --- .../{main.py => main_single_step_flat_ground.py} | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) rename src/hippopt/turnkey_planners/humanoid_kinodynamic/{main.py => main_single_step_flat_ground.py} (99%) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py similarity index 99% rename from src/hippopt/turnkey_planners/humanoid_kinodynamic/main.py rename to src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py index 39bdc956..43eaead7 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -440,7 +440,7 @@ def get_references( timestep_s=output.values.dt, time_multiplier=1.0, save=True, - file_name_stem="humanoid_walking", + file_name_stem="humanoid_walking_single_step", ) print("Press [Enter] to exit.") From 47ac89953bb6a3cbea2d8f5d0b9d787711489381 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 4 Nov 2023 19:07:33 +0100 Subject: [PATCH 071/100] Added possibility to visualize multiple states at once --- .../humanoid_kinodynamic/main_single_step_flat_ground.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py index 43eaead7..93a8ba52 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -432,11 +432,13 @@ def get_references( ) visualizer.visualize( - state=humanoid_states, timestep_s=output.values.dt, time_multiplier=2.0 + states=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=2.0, ) visualizer.visualize( - state=humanoid_states, + states=humanoid_states, timestep_s=output.values.dt, time_multiplier=1.0, save=True, From 070f3078020ac416ed137dd30ece5e71642d5a9d Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 24 Nov 2023 16:40:07 +0100 Subject: [PATCH 072/100] Improved conversion of ExtendedHumanoid to HumanoidState --- .../humanoid_kinodynamic/planner.py | 22 ++++++++++++++++--- 1 file changed, 19 insertions(+), 3 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index b564a2fa..3a626c44 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -241,12 +241,29 @@ def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: hp_rp.ContactPointStateDerivative.__post_init__(self) self.u_v = np.zeros(3) + def to_contact_point_state(self) -> hp_rp.ContactPointState: + output = hp_rp.ContactPointState() + output.p = self.p + output.f = self.f + output.descriptor = self.descriptor + return output + @dataclasses.dataclass class FeetContactPointsExtended(hp.OptimizationObject): left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + def to_feet_contact_points(self) -> hp_rp.FeetContactPoints: + output = hp_rp.FeetContactPoints() + output.left = hp_rp.FootContactState.from_list( + [point.to_contact_point_state() for point in self.left] + ) + output.right = hp_rp.FootContactState.from_list( + [point.to_contact_point_state() for point in self.right] + ) + return output + @dataclasses.dataclass class ExtendedHumanoid(hp.OptimizationObject): @@ -287,9 +304,8 @@ def __post_init__( def to_humanoid_state(self) -> hp_rp.HumanoidState: output = hp_rp.HumanoidState() - output.kinematics.base = self.kinematics.base - output.kinematics.joints = self.kinematics.joints - output.contact_points = self.contact_points + output.kinematics = self.kinematics.to_floating_base_system_state() + output.contact_points = self.contact_points.to_feet_contact_points() output.com = self.com output.centroidal_momentum = self.centroidal_momentum return output From bbcbf1516ea09a00b3574ead2354678c24af7072 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 24 Nov 2023 18:16:39 +0100 Subject: [PATCH 073/100] Removed centroidal momentum from the HumanoidState --- .../main_single_step_flat_ground.py | 11 +++++--- .../humanoid_kinodynamic/planner.py | 26 +++++++++++++++---- .../humanoid_pose_finder/planner.py | 1 - 3 files changed, 29 insertions(+), 9 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py index 93a8ba52..64503bd2 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -192,7 +192,7 @@ def get_visualizer_settings( def compute_initial_state( input_settings: walking_planner.Settings, pf_input: pose_finder.Planner, -) -> hp_rp.HumanoidState: +) -> walking_planner.ExtendedHumanoidState: desired_joints = np.deg2rad( [ 7, @@ -259,9 +259,14 @@ def compute_initial_state( output_pf = pf_input.solve() - output_pf.values.state.centroidal_momentum = np.zeros((6, 1)) + output_state = walking_planner.ExtendedHumanoidState() + output_state.contact_points = output_pf.values.state.contact_points + output_state.kinematics = output_pf.values.state.kinematics + output_state.com = output_pf.values.state.com - return output_pf.values.state + output_state.centroidal_momentum = np.zeros((6, 1)) + + return output_state def compute_final_state( diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 3a626c44..bdd1c2f3 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -307,10 +307,26 @@ def to_humanoid_state(self) -> hp_rp.HumanoidState: output.kinematics = self.kinematics.to_floating_base_system_state() output.contact_points = self.contact_points.to_feet_contact_points() output.com = self.com - output.centroidal_momentum = self.centroidal_momentum return output +@dataclasses.dataclass +class ExtendedHumanoidState(hp_rp.HumanoidState): + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + hp_rp.HumanoidState.__post_init__( + self, + contact_point_descriptors=contact_point_descriptors, + number_of_joints=number_of_joints, + ) + self.centroidal_momentum = np.zeros(6) + + @dataclasses.dataclass class Variables(hp.OptimizationObject): system: hp.CompositeType[ExtendedHumanoid] = hp.default_composite_field( @@ -319,8 +335,8 @@ class Variables(hp.OptimizationObject): mass: hp.StorageType = hp.default_storage_field(hp.Parameter) - initial_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( - cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False + initial_state: hp.CompositeType[ExtendedHumanoidState] = hp.default_composite_field( + cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False ) dt: hp.StorageType = hp.default_storage_field(hp.Parameter) @@ -365,7 +381,7 @@ def __post_init__( number_of_joints=kin_dyn_object.NDoF, ) - self.initial_state = hp_rp.HumanoidState( + self.initial_state = ExtendedHumanoidState( contact_point_descriptors=settings.contact_points, number_of_joints=kin_dyn_object.NDoF, ) @@ -1184,7 +1200,7 @@ def set_references(self, references: References | list[References]) -> None: ) self.ocp.problem.set_initial_guess(guess) - def set_initial_state(self, initial_state: hp_rp.HumanoidState) -> None: + def set_initial_state(self, initial_state: ExtendedHumanoidState) -> None: guess = self.get_initial_guess() guess.initial_state = initial_state guess.initial_state.centroidal_momentum /= self.numeric_mass diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index b43bc159..7cd0e831 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -287,7 +287,6 @@ def get_function_inputs_dict(self): variables = self.op.variables function_inputs = { "mass_name": variables.mass.name(), - "momentum_name": variables.state.centroidal_momentum.name(), "com_name": variables.state.com.name(), "quaternion_xyzw_name": "q", "gravity_name": variables.gravity.name(), From ddd89da6a4752a51d2329b60f5d3cd64b14407e6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 24 Nov 2023 18:28:28 +0100 Subject: [PATCH 074/100] Added file to perform periodic motions with the kinodynamic planner --- .../main_periodic_step.py | 460 ++++++++++++++++++ .../humanoid_kinodynamic/planner.py | 45 ++ 2 files changed, 505 insertions(+) create mode 100644 src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py new file mode 100644 index 00000000..628e9c81 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -0,0 +1,460 @@ +import argparse +import logging + +import casadi as cs +import idyntree.bindings as idyntree +import liecasadi +import numpy as np + +import hippopt +import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_kinodynamic.planner as walking_planner +import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder + + +def get_planner_settings() -> walking_planner.Settings: + parser = argparse.ArgumentParser( + description="Trajectory Optimization of a forward walking motion on ergoCub.", + ) + parser.add_argument( + "--urdf", + type=str, + required=True, + help="Path to the ergoCubGazeboV1_minContacts URDF file.", + ) + settings = walking_planner.Settings() + settings.robot_urdf = parser.parse_args().urdf + 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 = 90.0 + settings.base_quaternion_cost_multiplier = 50.0 + settings.base_quaternion_velocity_cost_multiplier = 0.001 + settings.joint_regularization_cost_multiplier = 0.1 + settings.force_regularization_cost_multiplier = 10.0 + settings.foot_yaw_regularization_cost_multiplier = 2000.0 + settings.swing_foot_height_cost_multiplier = 1000.0 + settings.contact_velocity_control_cost_multiplier = 5.0 + settings.contact_force_control_cost_multiplier = 0.0001 + settings.final_state_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": 1e0, + "acceptable_iter": 2, + "acceptable_compl_inf_tol": 1.0, + "warm_start_bound_frac": 1e-2, + "warm_start_bound_push": 1e-2, + "warm_start_mult_bound_push": 1e-2, + "warm_start_slack_bound_frac": 1e-2, + "warm_start_slack_bound_push": 1e-2, + "warm_start_init_point": "yes", + "required_infeasibility_reduction": 0.8, + "perturb_dec_fact": 0.1, + "max_hessian_perturbation": 100.0, + "acceptable_obj_change_tol": 1e0, + } + + return settings + + +def get_pose_finder_settings( + input_settings: walking_planner.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_planner.Settings, +) -> hp_rp.HumanoidStateVisualizerSettings: + output_viz_settings = hp_rp.HumanoidStateVisualizerSettings() + output_viz_settings.robot_model = input_settings.robot_urdf + output_viz_settings.considered_joints = input_settings.joints_name_list + output_viz_settings.contact_points = input_settings.contact_points + output_viz_settings.terrain = input_settings.terrain + output_viz_settings.working_folder = "./" + + return output_viz_settings + + +def compute_initial_state( + input_settings: walking_planner.Settings, + pf_input: pose_finder.Planner, +) -> walking_planner.ExtendedHumanoidState: + desired_joints = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + assert len(input_settings.joints_name_list) == len(desired_joints) + + pf_ref = pose_finder.References( + contact_point_descriptors=pf_settings.contact_points, + number_of_joints=len(desired_joints), + ) + + pf_ref.state.com = np.array([0.0, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + pf_ref.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + pf_ref.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + pf_ref.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + + pf_ref.state.kinematics.joints.positions = desired_joints + + pf_input.set_references(pf_ref) + + output_pf = pf_input.solve() + + output_state = walking_planner.ExtendedHumanoidState() + output_state.contact_points = output_pf.values.state.contact_points + output_state.kinematics = output_pf.values.state.kinematics + output_state.com = output_pf.values.state.com + + output_state.centroidal_momentum = np.zeros((6, 1)) + + return output_state + + +def compute_final_state( + input_settings: walking_planner.Settings, + pf_input: pose_finder.Planner, +) -> hp_rp.HumanoidState: + desired_joints = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + assert len(input_settings.joints_name_list) == len(desired_joints) + + pf_ref = pose_finder.References( + contact_point_descriptors=pf_settings.contact_points, + number_of_joints=len(desired_joints), + ) + + pf_ref.state.com = np.array([0.15, 0.0, 0.7]) + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + pf_ref.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + pf_ref.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + pf_ref.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + + pf_ref.state.kinematics.joints.positions = desired_joints + + pf_input.set_references(pf_ref) + + output_pf = pf_input.solve() + return output_pf.values.state + + +def get_references( + input_settings: walking_planner.Settings, desired_state: hp_rp.HumanoidState +) -> walking_planner.References: + output_reference = walking_planner.References( + number_of_joints=len(input_settings.joints_name_list), + number_of_points_left=len(input_settings.contact_points.left), + number_of_points_right=len(input_settings.contact_points.right), + ) + + output_reference.contacts_centroid_cost_weights = [100, 100, 10] + output_reference.contacts_centroid = [0.3, 0.0, 0.0] + output_reference.joint_regularization = desired_state.kinematics.joints.positions + output_reference.com_linear_velocity = [0.1, 0.0, 0.0] + + return output_reference + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + + planner_settings = get_planner_settings() + planner = walking_planner.Planner(settings=planner_settings) + + pf_settings = get_pose_finder_settings(input_settings=planner_settings) + pf = pose_finder.Planner(settings=pf_settings) + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + + initial_state = compute_initial_state( + input_settings=planner_settings, + pf_input=pf, + ) + visualizer.visualize(initial_state) + + planner.set_initial_state(initial_state) + + final_state = compute_final_state( + input_settings=planner_settings, + pf_input=pf, + ) + final_state.centroidal_momentum = np.zeros((6, 1)) + + print("Press [Enter] to visualize the final desired state.") + input() + + visualizer.visualize(final_state) + + print("Starting the planner...") + + references = get_references( + input_settings=planner_settings, + desired_state=final_state, + ) + + planner.set_references(references) + planner.set_final_state(final_state) + + output = planner.solve() + + humanoid_states = [s.to_humanoid_state() for s in output.values.system] + left_contact_points = [s.contact_points.left for s in humanoid_states] + right_contact_points = [s.contact_points.right for s in humanoid_states] + print("Press [Enter] to visualize the solution.") + input() + + plotter_settings = hp_rp.FootContactStatePlotterSettings() + plotter_settings.terrain = planner_settings.terrain + left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + + left_foot_plotter.plot_complementarity( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Complementarity", + blocking=False, + ) + right_foot_plotter.plot_complementarity( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Complementarity", + blocking=False, + ) + + left_foot_plotter.plot_forces( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Forces", + blocking=False, + ) + right_foot_plotter.plot_forces( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Forces", + blocking=False, + ) + + visualizer.visualize( + states=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=2.0, + ) + + visualizer.visualize( + states=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_single_step", + ) + + print("Press [Enter] to exit.") + input() + left_foot_plotter.close() + right_foot_plotter.close() diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index bdd1c2f3..72b735e2 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -37,6 +37,10 @@ class Settings: maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) + final_state_expression_type: hp.ExpressionType = dataclasses.field(default=None) + + final_state_expression_weight: float = dataclasses.field(default=None) + contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) @@ -85,6 +89,8 @@ def __post_init__(self) -> None: 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 def is_valid(self) -> bool: number_of_joints = len(self.joints_name_list) @@ -339,6 +345,10 @@ class Variables(hp.OptimizationObject): cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False ) + final_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False + ) + dt: hp.StorageType = hp.default_storage_field(hp.Parameter) gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) planar_dcc_height_multiplier: hp.StorageType = hp.default_storage_field( @@ -386,6 +396,11 @@ def __post_init__( number_of_joints=kin_dyn_object.NDoF, ) + self.final_state = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + self.dt = settings.time_step self.gravity = kin_dyn_object.g self.mass = kin_dyn_object.get_total_mass() @@ -735,6 +750,26 @@ def add_kinematics_constraints( name="joint_velocity_bounds", ) + # Final state + state_vectorized = sym.system.to_humanoid_state().to_list() + final_state_vectorized = ( + problem.final(state_el) for state_el in state_vectorized + ) + desired_final_state = sym.final_state.to_list() + desired_final_state_vectorized = ( + problem.initial(state_el) for state_el in desired_final_state + ) + + problem.add_expression( + mode=self.settings.final_state_expression_type, + expression=cs.MX( + cs.vertcat(*final_state_vectorized) + == cs.vertcat(*desired_final_state_vectorized) + ), + name="final_state_expression", + scaling=self.settings.final_state_expression_weight, + ) + def add_kinematics_regularization( self, function_inputs: dict, base_quaternion_normalized: cs.MX ): @@ -1211,6 +1246,16 @@ def set_initial_state(self, initial_state: ExtendedHumanoidState) -> None: point.f /= self.numeric_mass self.ocp.problem.set_initial_guess(guess) + def set_final_state(self, final_state: hp_rp.HumanoidState) -> None: + guess = self.get_initial_guess() + guess.final_state = final_state + for point in ( + guess.final_state.contact_points.left + + guess.final_state.contact_points.right + ): + point.f /= self.numeric_mass + self.ocp.problem.set_initial_guess(guess) + def solve(self) -> hp.Output[Variables]: output = self.ocp.problem.solve() From cee4718f82dab105f84a56a10a42be359576bd0d Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 10:34:11 +0100 Subject: [PATCH 075/100] Moved kinodynamic planner settings to a different file --- src/hippopt/turnkey_planners/__init__.py | 0 .../humanoid_kinodynamic/__init__.py | 0 .../main_periodic_step.py | 16 ++- .../main_single_step_flat_ground.py | 16 ++- .../humanoid_kinodynamic/planner.py | 122 +---------------- .../humanoid_kinodynamic/settings.py | 127 ++++++++++++++++++ 6 files changed, 146 insertions(+), 135 deletions(-) create mode 100644 src/hippopt/turnkey_planners/__init__.py create mode 100644 src/hippopt/turnkey_planners/humanoid_kinodynamic/__init__.py create mode 100644 src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py diff --git a/src/hippopt/turnkey_planners/__init__.py b/src/hippopt/turnkey_planners/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/__init__.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/__init__.py new file mode 100644 index 00000000..e69de29b diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 628e9c81..7fac5411 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -9,10 +9,11 @@ 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_pose_finder.planner as pose_finder -def get_planner_settings() -> walking_planner.Settings: +def get_planner_settings() -> walking_settings.Settings: parser = argparse.ArgumentParser( description="Trajectory Optimization of a forward walking motion on ergoCub.", ) @@ -22,7 +23,7 @@ def get_planner_settings() -> walking_planner.Settings: required=True, help="Path to the ergoCubGazeboV1_minContacts URDF file.", ) - settings = walking_planner.Settings() + settings = walking_settings.Settings() settings.robot_urdf = parser.parse_args().urdf settings.joints_name_list = [ "torso_pitch", @@ -139,7 +140,7 @@ def get_planner_settings() -> walking_planner.Settings: def get_pose_finder_settings( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, ) -> pose_finder.Settings: number_of_joints = len(input_settings.joints_name_list) settings = pose_finder.Settings() @@ -179,7 +180,7 @@ def get_pose_finder_settings( def get_visualizer_settings( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, ) -> hp_rp.HumanoidStateVisualizerSettings: output_viz_settings = hp_rp.HumanoidStateVisualizerSettings() output_viz_settings.robot_model = input_settings.robot_urdf @@ -192,7 +193,7 @@ def get_visualizer_settings( def compute_initial_state( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, ) -> walking_planner.ExtendedHumanoidState: desired_joints = np.deg2rad( @@ -272,7 +273,7 @@ def compute_initial_state( def compute_final_state( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, ) -> hp_rp.HumanoidState: desired_joints = np.deg2rad( @@ -344,7 +345,8 @@ def compute_final_state( def get_references( - input_settings: walking_planner.Settings, desired_state: hp_rp.HumanoidState + input_settings: walking_settings.Settings, + desired_state: hp_rp.HumanoidState, ) -> walking_planner.References: output_reference = walking_planner.References( number_of_joints=len(input_settings.joints_name_list), diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py index 64503bd2..8f949a5e 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -8,10 +8,11 @@ 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_pose_finder.planner as pose_finder -def get_planner_settings() -> walking_planner.Settings: +def get_planner_settings() -> walking_settings.Settings: parser = argparse.ArgumentParser( description="Trajectory Optimization of a forward walking motion on ergoCub.", ) @@ -21,7 +22,7 @@ def get_planner_settings() -> walking_planner.Settings: required=True, help="Path to the ergoCubGazeboV1_minContacts URDF file.", ) - settings = walking_planner.Settings() + settings = walking_settings.Settings() settings.robot_urdf = parser.parse_args().urdf settings.joints_name_list = [ "torso_pitch", @@ -137,7 +138,7 @@ def get_planner_settings() -> walking_planner.Settings: def get_pose_finder_settings( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, ) -> pose_finder.Settings: number_of_joints = len(input_settings.joints_name_list) settings = pose_finder.Settings() @@ -177,7 +178,7 @@ def get_pose_finder_settings( def get_visualizer_settings( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, ) -> hp_rp.HumanoidStateVisualizerSettings: output_viz_settings = hp_rp.HumanoidStateVisualizerSettings() output_viz_settings.robot_model = input_settings.robot_urdf @@ -190,7 +191,7 @@ def get_visualizer_settings( def compute_initial_state( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, ) -> walking_planner.ExtendedHumanoidState: desired_joints = np.deg2rad( @@ -270,7 +271,7 @@ def compute_initial_state( def compute_final_state( - input_settings: walking_planner.Settings, + input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, ) -> hp_rp.HumanoidState: desired_joints = np.deg2rad( @@ -342,7 +343,8 @@ def compute_final_state( def get_references( - input_settings: walking_planner.Settings, desired_state: hp_rp.HumanoidState + input_settings: walking_settings.Settings, + desired_state: hp_rp.HumanoidState, ) -> walking_planner.References: output_reference = walking_planner.References( number_of_joints=len(input_settings.joints_name_list), diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 72b735e2..b7b2a671 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -1,133 +1,13 @@ import copy import dataclasses -import typing import adam.casadi import casadi as cs import numpy as np import hippopt as hp -import hippopt.integrators as hp_int import hippopt.robot_planning as hp_rp - - -@dataclasses.dataclass -class Settings: - robot_urdf: str = dataclasses.field(default=None) - joints_name_list: list[str] = dataclasses.field(default=None) - contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) - root_link: str = dataclasses.field(default=None) - gravity: np.array = dataclasses.field(default=None) - horizon_length: int = dataclasses.field(default=None) - time_step: float = dataclasses.field(default=None) - integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) - terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) - planar_dcc_height_multiplier: float = dataclasses.field(default=None) - dcc_gain: float = dataclasses.field(default=None) - dcc_epsilon: float = dataclasses.field(default=None) - static_friction: float = dataclasses.field(default=None) - maximum_velocity_control: np.ndarray = dataclasses.field(default=None) - maximum_force_derivative: np.ndarray = dataclasses.field(default=None) - maximum_angular_momentum: float = dataclasses.field(default=None) - minimum_com_height: float = dataclasses.field(default=None) - minimum_feet_lateral_distance: float = dataclasses.field(default=None) - maximum_feet_relative_height: float = dataclasses.field(default=None) - maximum_joint_positions: np.ndarray = dataclasses.field(default=None) - minimum_joint_positions: np.ndarray = dataclasses.field(default=None) - maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) - minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) - - final_state_expression_type: hp.ExpressionType = dataclasses.field(default=None) - - final_state_expression_weight: float = dataclasses.field(default=None) - - contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) - - com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) - com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) - - desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) - - desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) - - base_quaternion_cost_multiplier: float = dataclasses.field(default=None) - - base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) - - joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) - joint_regularization_cost_multiplier: float = dataclasses.field(default=None) - - force_regularization_cost_multiplier: float = dataclasses.field(default=None) - - foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) - - swing_foot_height_cost_multiplier: float = dataclasses.field(default=None) - - contact_velocity_control_cost_multiplier: float = dataclasses.field(default=None) - - contact_force_control_cost_multiplier: float = dataclasses.field(default=None) - - opti_solver: str = dataclasses.field(default="ipopt") - - problem_type: str = dataclasses.field(default="nlp") - - casadi_function_options: dict = dataclasses.field(default_factory=dict) - - casadi_opti_options: dict = dataclasses.field(default_factory=dict) - - casadi_solver_options: dict = dataclasses.field(default_factory=dict) - - def __post_init__(self) -> None: - self.root_link = "root_link" - self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) - self.integrator = hp_int.ImplicitTrapezoid - self.terrain = hp_rp.PlanarTerrain() - self.planar_dcc_height_multiplier = 10.0 - self.dcc_gain = 20.0 - self.dcc_epsilon = 0.05 - self.static_friction = 0.3 - self.maximum_velocity_control = np.array([2.0, 2.0, 5.0]) - self.maximum_force_derivative = np.array([100.0, 100.0, 100.0]) - self.maximum_angular_momentum = 10.0 - self.final_state_expression_type = hp.ExpressionType.skip - self.final_state_expression_weight = 1.0 - - def is_valid(self) -> bool: - number_of_joints = len(self.joints_name_list) - return ( - self.robot_urdf is not None - and self.joints_name_list is not None - and self.contact_points is not None - and self.horizon_length is not None - and self.time_step is not None - and self.minimum_com_height is not None - and self.minimum_feet_lateral_distance is not None - and self.maximum_feet_relative_height is not None - and self.maximum_joint_positions is not None - and self.minimum_joint_positions is not None - and self.maximum_joint_velocities is not None - and self.minimum_joint_velocities is not None - and len(self.maximum_joint_positions) == number_of_joints - and len(self.minimum_joint_positions) == number_of_joints - and len(self.maximum_joint_velocities) == number_of_joints - and len(self.minimum_joint_velocities) == number_of_joints - and self.contacts_centroid_cost_multiplier is not None - and self.com_linear_velocity_cost_weights is not None - and len(self.com_linear_velocity_cost_weights) == 3 - and self.com_linear_velocity_cost_multiplier is not None - and self.desired_frame_quaternion_cost_frame_name is not None - and self.desired_frame_quaternion_cost_multiplier is not None - and self.base_quaternion_cost_multiplier is not None - and self.base_quaternion_velocity_cost_multiplier is not None - and self.joint_regularization_cost_weights is not None - and len(self.joint_regularization_cost_weights) == number_of_joints - and self.joint_regularization_cost_multiplier is not None - and self.force_regularization_cost_multiplier is not None - and self.foot_yaw_regularization_cost_multiplier is not None - and self.swing_foot_height_cost_multiplier is not None - and self.contact_velocity_control_cost_multiplier is not None - and self.contact_force_control_cost_multiplier is not None - ) +from hippopt.turnkey_planners.humanoid_kinodynamic.settings import Settings @dataclasses.dataclass diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py new file mode 100644 index 00000000..76ab1032 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -0,0 +1,127 @@ +import dataclasses +import typing + +import numpy as np + +import hippopt as hp +from hippopt import integrators as hp_int +from hippopt import robot_planning as hp_rp + + +@dataclasses.dataclass +class Settings: + robot_urdf: str = dataclasses.field(default=None) + joints_name_list: list[str] = dataclasses.field(default=None) + contact_points: hp_rp.FeetContactPointDescriptors = dataclasses.field(default=None) + root_link: str = dataclasses.field(default=None) + gravity: np.array = dataclasses.field(default=None) + horizon_length: int = dataclasses.field(default=None) + time_step: float = dataclasses.field(default=None) + integrator: typing.Type[hp.SingleStepIntegrator] = dataclasses.field(default=None) + terrain: hp_rp.TerrainDescriptor = dataclasses.field(default=None) + planar_dcc_height_multiplier: float = dataclasses.field(default=None) + dcc_gain: float = dataclasses.field(default=None) + dcc_epsilon: float = dataclasses.field(default=None) + static_friction: float = dataclasses.field(default=None) + maximum_velocity_control: np.ndarray = dataclasses.field(default=None) + maximum_force_derivative: np.ndarray = dataclasses.field(default=None) + maximum_angular_momentum: float = dataclasses.field(default=None) + minimum_com_height: float = dataclasses.field(default=None) + minimum_feet_lateral_distance: float = dataclasses.field(default=None) + maximum_feet_relative_height: float = dataclasses.field(default=None) + maximum_joint_positions: np.ndarray = dataclasses.field(default=None) + minimum_joint_positions: np.ndarray = dataclasses.field(default=None) + maximum_joint_velocities: np.ndarray = dataclasses.field(default=None) + minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) + + final_state_expression_type: hp.ExpressionType = dataclasses.field(default=None) + + final_state_expression_weight: float = dataclasses.field(default=None) + + contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) + + com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) + com_linear_velocity_cost_multiplier: float = dataclasses.field(default=None) + + desired_frame_quaternion_cost_frame_name: str = dataclasses.field(default=None) + + desired_frame_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + base_quaternion_cost_multiplier: float = dataclasses.field(default=None) + + base_quaternion_velocity_cost_multiplier: float = dataclasses.field(default=None) + + joint_regularization_cost_weights: np.ndarray = dataclasses.field(default=None) + joint_regularization_cost_multiplier: float = dataclasses.field(default=None) + + force_regularization_cost_multiplier: float = dataclasses.field(default=None) + + foot_yaw_regularization_cost_multiplier: float = dataclasses.field(default=None) + + swing_foot_height_cost_multiplier: float = dataclasses.field(default=None) + + contact_velocity_control_cost_multiplier: float = dataclasses.field(default=None) + + contact_force_control_cost_multiplier: float = dataclasses.field(default=None) + + opti_solver: str = dataclasses.field(default="ipopt") + + problem_type: str = dataclasses.field(default="nlp") + + casadi_function_options: dict = dataclasses.field(default_factory=dict) + + casadi_opti_options: dict = dataclasses.field(default_factory=dict) + + casadi_solver_options: dict = dataclasses.field(default_factory=dict) + + def __post_init__(self) -> None: + self.root_link = "root_link" + self.gravity = np.array([0.0, 0.0, -9.80665, 0.0, 0.0, 0.0]) + self.integrator = hp_int.ImplicitTrapezoid + self.terrain = hp_rp.PlanarTerrain() + self.planar_dcc_height_multiplier = 10.0 + self.dcc_gain = 20.0 + self.dcc_epsilon = 0.05 + self.static_friction = 0.3 + self.maximum_velocity_control = np.array([2.0, 2.0, 5.0]) + self.maximum_force_derivative = np.array([100.0, 100.0, 100.0]) + self.maximum_angular_momentum = 10.0 + self.final_state_expression_type = hp.ExpressionType.skip + self.final_state_expression_weight = 1.0 + + def is_valid(self) -> bool: + number_of_joints = len(self.joints_name_list) + return ( + self.robot_urdf is not None + and self.joints_name_list is not None + and self.contact_points is not None + and self.horizon_length is not None + and self.time_step is not None + and self.minimum_com_height is not None + and self.minimum_feet_lateral_distance is not None + and self.maximum_feet_relative_height is not None + and self.maximum_joint_positions is not None + and self.minimum_joint_positions is not None + and self.maximum_joint_velocities is not None + and self.minimum_joint_velocities is not None + and len(self.maximum_joint_positions) == number_of_joints + and len(self.minimum_joint_positions) == number_of_joints + and len(self.maximum_joint_velocities) == number_of_joints + and len(self.minimum_joint_velocities) == number_of_joints + and self.contacts_centroid_cost_multiplier is not None + and self.com_linear_velocity_cost_weights is not None + and len(self.com_linear_velocity_cost_weights) == 3 + and self.com_linear_velocity_cost_multiplier is not None + and self.desired_frame_quaternion_cost_frame_name is not None + and self.desired_frame_quaternion_cost_multiplier is not None + and self.base_quaternion_cost_multiplier is not None + and self.base_quaternion_velocity_cost_multiplier is not None + and self.joint_regularization_cost_weights is not None + and len(self.joint_regularization_cost_weights) == number_of_joints + and self.joint_regularization_cost_multiplier is not None + and self.force_regularization_cost_multiplier is not None + and self.foot_yaw_regularization_cost_multiplier is not None + and self.swing_foot_height_cost_multiplier is not None + and self.contact_velocity_control_cost_multiplier is not None + and self.contact_force_control_cost_multiplier is not None + ) From e73ec1eae6904989781ac8d916f108fe420f114d Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 10:39:32 +0100 Subject: [PATCH 076/100] Moved kinodynamic planner variables to a different file --- .../main_periodic_step.py | 9 +- .../main_single_step_flat_ground.py | 9 +- .../humanoid_kinodynamic/planner.py | 304 +----------------- .../humanoid_kinodynamic/variables.py | 304 ++++++++++++++++++ 4 files changed, 321 insertions(+), 305 deletions(-) create mode 100644 src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 7fac5411..be1de061 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -10,6 +10,7 @@ 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 @@ -195,7 +196,7 @@ def get_visualizer_settings( def compute_initial_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, -) -> walking_planner.ExtendedHumanoidState: +) -> walking_variables.ExtendedHumanoidState: desired_joints = np.deg2rad( [ 7, @@ -262,7 +263,7 @@ def compute_initial_state( output_pf = pf_input.solve() - output_state = walking_planner.ExtendedHumanoidState() + output_state = walking_variables.ExtendedHumanoidState() output_state.contact_points = output_pf.values.state.contact_points output_state.kinematics = output_pf.values.state.kinematics output_state.com = output_pf.values.state.com @@ -347,8 +348,8 @@ def compute_final_state( def get_references( input_settings: walking_settings.Settings, desired_state: hp_rp.HumanoidState, -) -> walking_planner.References: - output_reference = walking_planner.References( +) -> walking_variables.References: + output_reference = walking_variables.References( number_of_joints=len(input_settings.joints_name_list), number_of_points_left=len(input_settings.contact_points.left), number_of_points_right=len(input_settings.contact_points.right), diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py index 8f949a5e..8dca28b1 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -9,6 +9,7 @@ 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 @@ -193,7 +194,7 @@ def get_visualizer_settings( def compute_initial_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, -) -> walking_planner.ExtendedHumanoidState: +) -> walking_variables.ExtendedHumanoidState: desired_joints = np.deg2rad( [ 7, @@ -260,7 +261,7 @@ def compute_initial_state( output_pf = pf_input.solve() - output_state = walking_planner.ExtendedHumanoidState() + output_state = walking_variables.ExtendedHumanoidState() output_state.contact_points = output_pf.values.state.contact_points output_state.kinematics = output_pf.values.state.kinematics output_state.com = output_pf.values.state.com @@ -345,8 +346,8 @@ def compute_final_state( def get_references( input_settings: walking_settings.Settings, desired_state: hp_rp.HumanoidState, -) -> walking_planner.References: - output_reference = walking_planner.References( +) -> walking_variables.References: + output_reference = walking_variables.References( number_of_joints=len(input_settings.joints_name_list), number_of_points_left=len(input_settings.contact_points.left), number_of_points_right=len(input_settings.contact_points.right), diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index b7b2a671..187a5abb 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -1,5 +1,4 @@ import copy -import dataclasses import adam.casadi import casadi as cs @@ -8,302 +7,13 @@ import hippopt as hp import hippopt.robot_planning as hp_rp from hippopt.turnkey_planners.humanoid_kinodynamic.settings import Settings - - -@dataclasses.dataclass -class ContactReferences(hp.OptimizationObject): - desired_force_ratio: hp.StorageType = hp.default_storage_field(hp.Parameter) - - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) - - def __post_init__(self, number_of_points: int) -> None: - self.desired_force_ratio = ( - 1.0 / number_of_points - if number_of_points is not None and number_of_points > 0 - else 0.0 - ) - - -@dataclasses.dataclass -class FootReferences(hp.OptimizationObject): - points: hp.CompositeType[list[ContactReferences]] = hp.default_composite_field( - factory=list, time_varying=False - ) - yaw: hp.StorageType = hp.default_storage_field(hp.Parameter) - - number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=0) - - def __post_init__(self, number_of_points: int) -> None: - number_of_points = number_of_points if number_of_points is not None else 0 - self.points = [ - ContactReferences(number_of_points=number_of_points) - for _ in range(number_of_points) - ] - - self.yaw = 0.0 - - -@dataclasses.dataclass -class FeetReferences(hp.OptimizationObject): - left: hp.CompositeType[FootReferences] = hp.default_composite_field( - factory=FootReferences, time_varying=False - ) - right: hp.CompositeType[FootReferences] = hp.default_composite_field( - factory=FootReferences, time_varying=False - ) - - desired_swing_height: hp.StorageType = hp.default_storage_field(hp.Parameter) - - number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) - number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) - - def __post_init__( - self, number_of_points_left: int, number_of_points_right: int - ) -> None: - self.left = FootReferences(number_of_points=number_of_points_left) - self.right = FootReferences(number_of_points=number_of_points_right) - self.desired_swing_height = 0.02 - - -@dataclasses.dataclass -class References(hp.OptimizationObject): - feet: hp.CompositeType[FeetReferences] = hp.default_composite_field( - factory=FeetReferences, time_varying=False - ) - - contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( - hp.Parameter - ) - contacts_centroid: hp.StorageType = hp.default_storage_field(hp.Parameter) - - com_linear_velocity: hp.StorageType = hp.default_storage_field(hp.Parameter) - - desired_frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field( - hp.Parameter - ) - - base_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) - - base_quaternion_xyzw_velocity: hp.StorageType = hp.default_storage_field( - hp.Parameter - ) - - joint_regularization: hp.StorageType = hp.default_storage_field(hp.Parameter) - - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=0) - number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) - number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) - - def __post_init__( - self, - number_of_joints: int, - number_of_points_left: int, - number_of_points_right: int, - ) -> None: - self.feet = FeetReferences( - number_of_points_left=number_of_points_left, - number_of_points_right=number_of_points_right, - ) - self.contacts_centroid_cost_weights = np.zeros((3, 1)) - self.contacts_centroid = np.zeros((3, 1)) - self.com_linear_velocity = np.zeros((3, 1)) - self.desired_frame_quaternion_xyzw = np.zeros((4, 1)) - self.desired_frame_quaternion_xyzw[3] = 1 - self.base_quaternion_xyzw = np.zeros((4, 1)) - self.base_quaternion_xyzw[3] = 1 - self.base_quaternion_xyzw_velocity = np.zeros((4, 1)) - self.joint_regularization = np.zeros((number_of_joints, 1)) - - -@dataclasses.dataclass -class ExtendedContactPoint( - hp_rp.ContactPointState, - hp_rp.ContactPointStateDerivative, -): - u_v: hp.StorageType = hp.default_storage_field(hp.Variable) - - def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: - hp_rp.ContactPointState.__post_init__(self, input_descriptor) - hp_rp.ContactPointStateDerivative.__post_init__(self) - self.u_v = np.zeros(3) - - def to_contact_point_state(self) -> hp_rp.ContactPointState: - output = hp_rp.ContactPointState() - output.p = self.p - output.f = self.f - output.descriptor = self.descriptor - return output - - -@dataclasses.dataclass -class FeetContactPointsExtended(hp.OptimizationObject): - left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) - right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) - - def to_feet_contact_points(self) -> hp_rp.FeetContactPoints: - output = hp_rp.FeetContactPoints() - output.left = hp_rp.FootContactState.from_list( - [point.to_contact_point_state() for point in self.left] - ) - output.right = hp_rp.FootContactState.from_list( - [point.to_contact_point_state() for point in self.right] - ) - return output - - -@dataclasses.dataclass -class ExtendedHumanoid(hp.OptimizationObject): - contact_points: hp.CompositeType[ - FeetContactPointsExtended - ] = hp.default_composite_field(factory=FeetContactPointsExtended) - - kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( - cls=hp.Variable, factory=hp_rp.FloatingBaseSystem - ) - - com: hp.StorageType = hp.default_storage_field(hp.Variable) - centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) - - contact_point_descriptors: dataclasses.InitVar[ - hp_rp.FeetContactPointDescriptors - ] = dataclasses.field(default=None) - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) - - def __post_init__( - self, - contact_point_descriptors: hp_rp.FeetContactPointDescriptors, - number_of_joints: int, - ) -> None: - if contact_point_descriptors is not None: - self.contact_points.left = [ - ExtendedContactPoint(input_descriptor=point) - for point in contact_point_descriptors.left - ] - self.contact_points.right = [ - ExtendedContactPoint(input_descriptor=point) - for point in contact_point_descriptors.right - ] - - self.com = np.zeros(3) - self.centroidal_momentum = np.zeros(6) - self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) - - def to_humanoid_state(self) -> hp_rp.HumanoidState: - output = hp_rp.HumanoidState() - output.kinematics = self.kinematics.to_floating_base_system_state() - output.contact_points = self.contact_points.to_feet_contact_points() - output.com = self.com - return output - - -@dataclasses.dataclass -class ExtendedHumanoidState(hp_rp.HumanoidState): - centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) - - def __post_init__( - self, - contact_point_descriptors: hp_rp.FeetContactPointDescriptors, - number_of_joints: int, - ) -> None: - hp_rp.HumanoidState.__post_init__( - self, - contact_point_descriptors=contact_point_descriptors, - number_of_joints=number_of_joints, - ) - self.centroidal_momentum = np.zeros(6) - - -@dataclasses.dataclass -class Variables(hp.OptimizationObject): - system: hp.CompositeType[ExtendedHumanoid] = hp.default_composite_field( - cls=hp.Variable, factory=ExtendedHumanoid - ) - - mass: hp.StorageType = hp.default_storage_field(hp.Parameter) - - initial_state: hp.CompositeType[ExtendedHumanoidState] = hp.default_composite_field( - cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False - ) - - final_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( - cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False - ) - - dt: hp.StorageType = hp.default_storage_field(hp.Parameter) - gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) - planar_dcc_height_multiplier: hp.StorageType = hp.default_storage_field( - hp.Parameter - ) - dcc_gain: hp.StorageType = hp.default_storage_field(hp.Parameter) - dcc_epsilon: hp.StorageType = hp.default_storage_field(hp.Parameter) - static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) - maximum_velocity_control: hp.StorageType = hp.default_storage_field(hp.Parameter) - maximum_force_derivative: hp.StorageType = hp.default_storage_field(hp.Parameter) - maximum_angular_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) - minimum_com_height: hp.StorageType = hp.default_storage_field(hp.Parameter) - minimum_feet_lateral_distance: hp.StorageType = hp.default_storage_field( - hp.Parameter - ) - maximum_feet_relative_height: hp.StorageType = hp.default_storage_field( - hp.Parameter - ) - maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) - minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) - maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) - minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) - - references: hp.CompositeType[References] = hp.default_composite_field( - factory=References, time_varying=True - ) - - settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) - kin_dyn_object: dataclasses.InitVar[ - adam.casadi.KinDynComputations - ] = dataclasses.field(default=None) - - def __post_init__( - self, - settings: Settings, - kin_dyn_object: adam.casadi.KinDynComputations, - ) -> None: - self.system = ExtendedHumanoid( - contact_point_descriptors=settings.contact_points, - number_of_joints=kin_dyn_object.NDoF, - ) - - self.initial_state = ExtendedHumanoidState( - contact_point_descriptors=settings.contact_points, - number_of_joints=kin_dyn_object.NDoF, - ) - - self.final_state = hp_rp.HumanoidState( - contact_point_descriptors=settings.contact_points, - number_of_joints=kin_dyn_object.NDoF, - ) - - self.dt = settings.time_step - self.gravity = kin_dyn_object.g - self.mass = kin_dyn_object.get_total_mass() - - self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier - self.dcc_gain = settings.dcc_gain - self.dcc_epsilon = settings.dcc_epsilon - self.static_friction = settings.static_friction - self.maximum_velocity_control = settings.maximum_velocity_control - self.maximum_force_derivative = settings.maximum_force_derivative - self.maximum_angular_momentum = settings.maximum_angular_momentum - self.minimum_com_height = settings.minimum_com_height - self.minimum_feet_lateral_distance = settings.minimum_feet_lateral_distance - self.maximum_feet_relative_height = settings.maximum_feet_relative_height - self.maximum_joint_positions = settings.maximum_joint_positions - self.minimum_joint_positions = settings.minimum_joint_positions - self.maximum_joint_velocities = settings.maximum_joint_velocities - self.minimum_joint_velocities = settings.minimum_joint_velocities - self.references = References( - number_of_joints=kin_dyn_object.NDoF, - number_of_points_left=len(settings.contact_points.left), - number_of_points_right=len(settings.contact_points.right), - ) +from hippopt.turnkey_planners.humanoid_kinodynamic.variables import ( + ExtendedContactPoint, + ExtendedHumanoidState, + FootReferences, + References, + Variables, +) class Planner: diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py new file mode 100644 index 00000000..096854af --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -0,0 +1,304 @@ +import dataclasses + +import adam.casadi +import numpy as np + +import hippopt as hp +from hippopt import robot_planning as hp_rp +from hippopt.turnkey_planners.humanoid_kinodynamic.settings import Settings + + +@dataclasses.dataclass +class ContactReferences(hp.OptimizationObject): + desired_force_ratio: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_points: int) -> None: + self.desired_force_ratio = ( + 1.0 / number_of_points + if number_of_points is not None and number_of_points > 0 + else 0.0 + ) + + +@dataclasses.dataclass +class FootReferences(hp.OptimizationObject): + points: hp.CompositeType[list[ContactReferences]] = hp.default_composite_field( + factory=list, time_varying=False + ) + yaw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points: dataclasses.InitVar[int] = dataclasses.field(default=0) + + def __post_init__(self, number_of_points: int) -> None: + number_of_points = number_of_points if number_of_points is not None else 0 + self.points = [ + ContactReferences(number_of_points=number_of_points) + for _ in range(number_of_points) + ] + + self.yaw = 0.0 + + +@dataclasses.dataclass +class FeetReferences(hp.OptimizationObject): + left: hp.CompositeType[FootReferences] = hp.default_composite_field( + factory=FootReferences, time_varying=False + ) + right: hp.CompositeType[FootReferences] = hp.default_composite_field( + factory=FootReferences, time_varying=False + ) + + desired_swing_height: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) + + def __post_init__( + self, number_of_points_left: int, number_of_points_right: int + ) -> None: + self.left = FootReferences(number_of_points=number_of_points_left) + self.right = FootReferences(number_of_points=number_of_points_right) + self.desired_swing_height = 0.02 + + +@dataclasses.dataclass +class References(hp.OptimizationObject): + feet: hp.CompositeType[FeetReferences] = hp.default_composite_field( + factory=FeetReferences, time_varying=False + ) + + contacts_centroid_cost_weights: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + contacts_centroid: hp.StorageType = hp.default_storage_field(hp.Parameter) + + com_linear_velocity: hp.StorageType = hp.default_storage_field(hp.Parameter) + + desired_frame_quaternion_xyzw: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + + base_quaternion_xyzw: hp.StorageType = hp.default_storage_field(hp.Parameter) + + base_quaternion_xyzw_velocity: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + + joint_regularization: hp.StorageType = hp.default_storage_field(hp.Parameter) + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_left: dataclasses.InitVar[int] = dataclasses.field(default=0) + number_of_points_right: dataclasses.InitVar[int] = dataclasses.field(default=0) + + def __post_init__( + self, + number_of_joints: int, + number_of_points_left: int, + number_of_points_right: int, + ) -> None: + self.feet = FeetReferences( + number_of_points_left=number_of_points_left, + number_of_points_right=number_of_points_right, + ) + self.contacts_centroid_cost_weights = np.zeros((3, 1)) + self.contacts_centroid = np.zeros((3, 1)) + self.com_linear_velocity = np.zeros((3, 1)) + self.desired_frame_quaternion_xyzw = np.zeros((4, 1)) + self.desired_frame_quaternion_xyzw[3] = 1 + self.base_quaternion_xyzw = np.zeros((4, 1)) + self.base_quaternion_xyzw[3] = 1 + self.base_quaternion_xyzw_velocity = np.zeros((4, 1)) + self.joint_regularization = np.zeros((number_of_joints, 1)) + + +@dataclasses.dataclass +class ExtendedContactPoint( + hp_rp.ContactPointState, + hp_rp.ContactPointStateDerivative, +): + u_v: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__(self, input_descriptor: hp_rp.ContactPointDescriptor) -> None: + hp_rp.ContactPointState.__post_init__(self, input_descriptor) + hp_rp.ContactPointStateDerivative.__post_init__(self) + self.u_v = np.zeros(3) + + def to_contact_point_state(self) -> hp_rp.ContactPointState: + output = hp_rp.ContactPointState() + output.p = self.p + output.f = self.f + output.descriptor = self.descriptor + return output + + +@dataclasses.dataclass +class FeetContactPointsExtended(hp.OptimizationObject): + left: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + right: list[ExtendedContactPoint] = hp.default_composite_field(factory=list) + + def to_feet_contact_points(self) -> hp_rp.FeetContactPoints: + output = hp_rp.FeetContactPoints() + output.left = hp_rp.FootContactState.from_list( + [point.to_contact_point_state() for point in self.left] + ) + output.right = hp_rp.FootContactState.from_list( + [point.to_contact_point_state() for point in self.right] + ) + return output + + +@dataclasses.dataclass +class ExtendedHumanoid(hp.OptimizationObject): + contact_points: hp.CompositeType[ + FeetContactPointsExtended + ] = hp.default_composite_field(factory=FeetContactPointsExtended) + + kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( + cls=hp.Variable, factory=hp_rp.FloatingBaseSystem + ) + + com: hp.StorageType = hp.default_storage_field(hp.Variable) + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + + contact_point_descriptors: dataclasses.InitVar[ + hp_rp.FeetContactPointDescriptors + ] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + if contact_point_descriptors is not None: + self.contact_points.left = [ + ExtendedContactPoint(input_descriptor=point) + for point in contact_point_descriptors.left + ] + self.contact_points.right = [ + ExtendedContactPoint(input_descriptor=point) + for point in contact_point_descriptors.right + ] + + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(6) + self.kinematics = hp_rp.FloatingBaseSystem(number_of_joints=number_of_joints) + + def to_humanoid_state(self) -> hp_rp.HumanoidState: + output = hp_rp.HumanoidState() + output.kinematics = self.kinematics.to_floating_base_system_state() + output.contact_points = self.contact_points.to_feet_contact_points() + output.com = self.com + return output + + +@dataclasses.dataclass +class ExtendedHumanoidState(hp_rp.HumanoidState): + centroidal_momentum: hp.StorageType = hp.default_storage_field(hp.Variable) + + def __post_init__( + self, + contact_point_descriptors: hp_rp.FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + hp_rp.HumanoidState.__post_init__( + self, + contact_point_descriptors=contact_point_descriptors, + number_of_joints=number_of_joints, + ) + self.centroidal_momentum = np.zeros(6) + + +@dataclasses.dataclass +class Variables(hp.OptimizationObject): + system: hp.CompositeType[ExtendedHumanoid] = hp.default_composite_field( + cls=hp.Variable, factory=ExtendedHumanoid + ) + + mass: hp.StorageType = hp.default_storage_field(hp.Parameter) + + initial_state: hp.CompositeType[ExtendedHumanoidState] = hp.default_composite_field( + cls=hp.Parameter, factory=ExtendedHumanoidState, time_varying=False + ) + + final_state: hp.CompositeType[hp_rp.HumanoidState] = hp.default_composite_field( + cls=hp.Parameter, factory=hp_rp.HumanoidState, time_varying=False + ) + + dt: hp.StorageType = hp.default_storage_field(hp.Parameter) + gravity: hp.StorageType = hp.default_storage_field(hp.Parameter) + planar_dcc_height_multiplier: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + dcc_gain: hp.StorageType = hp.default_storage_field(hp.Parameter) + dcc_epsilon: hp.StorageType = hp.default_storage_field(hp.Parameter) + static_friction: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_velocity_control: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_force_derivative: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_angular_momentum: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_com_height: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_feet_lateral_distance: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + maximum_feet_relative_height: hp.StorageType = hp.default_storage_field( + hp.Parameter + ) + maximum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) + maximum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) + minimum_joint_velocities: hp.StorageType = hp.default_storage_field(hp.Parameter) + + references: hp.CompositeType[References] = hp.default_composite_field( + factory=References, time_varying=True + ) + + settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) + kin_dyn_object: dataclasses.InitVar[ + adam.casadi.KinDynComputations + ] = dataclasses.field(default=None) + + def __post_init__( + self, + settings: Settings, + kin_dyn_object: adam.casadi.KinDynComputations, + ) -> None: + self.system = ExtendedHumanoid( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + + self.initial_state = ExtendedHumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + + self.final_state = hp_rp.HumanoidState( + contact_point_descriptors=settings.contact_points, + number_of_joints=kin_dyn_object.NDoF, + ) + + self.dt = settings.time_step + self.gravity = kin_dyn_object.g + self.mass = kin_dyn_object.get_total_mass() + + self.planar_dcc_height_multiplier = settings.planar_dcc_height_multiplier + self.dcc_gain = settings.dcc_gain + self.dcc_epsilon = settings.dcc_epsilon + self.static_friction = settings.static_friction + self.maximum_velocity_control = settings.maximum_velocity_control + self.maximum_force_derivative = settings.maximum_force_derivative + self.maximum_angular_momentum = settings.maximum_angular_momentum + self.minimum_com_height = settings.minimum_com_height + self.minimum_feet_lateral_distance = settings.minimum_feet_lateral_distance + self.maximum_feet_relative_height = settings.maximum_feet_relative_height + self.maximum_joint_positions = settings.maximum_joint_positions + self.minimum_joint_positions = settings.minimum_joint_positions + self.maximum_joint_velocities = settings.maximum_joint_velocities + self.minimum_joint_velocities = settings.minimum_joint_velocities + self.references = References( + number_of_joints=kin_dyn_object.NDoF, + number_of_points_left=len(settings.contact_points.left), + number_of_points_right=len(settings.contact_points.right), + ) From 524e581edfb038284ffbf68efe601f17aca45022 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 11:29:26 +0100 Subject: [PATCH 077/100] Added possibility to add periodicity constraint on the control inputs --- .../main_periodic_step.py | 11 ++--- .../humanoid_kinodynamic/planner.py | 41 ++++++++++++++++++- .../humanoid_kinodynamic/settings.py | 6 ++- 3 files changed, 51 insertions(+), 7 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index be1de061..0c90da39 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -110,6 +110,7 @@ def get_planner_settings() -> walking_settings.Settings: 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 = { @@ -231,12 +232,12 @@ def compute_initial_state( number_of_joints=len(desired_joints), ) - pf_ref.state.com = np.array([0.0, 0.0, 0.7]) + pf_ref.state.com = np.array([0.15, 0.0, 0.7]) desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() ) desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() ) pf_ref.state.contact_points.left = ( hp_rp.FootContactState.from_parent_frame_transform( @@ -311,12 +312,12 @@ def compute_final_state( number_of_joints=len(desired_joints), ) - pf_ref.state.com = np.array([0.15, 0.0, 0.7]) + pf_ref.state.com = np.array([0.75, 0.0, 0.7]) desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + np.array([0.60, 0.1, 0.0]), liecasadi.SO3.Identity() ) desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + np.array([0.90, -0.1, 0.0]), liecasadi.SO3.Identity() ) pf_ref.state.contact_points.left = ( hp_rp.FootContactState.from_parent_frame_transform( diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 187a5abb..7e6d4949 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -141,6 +141,8 @@ def __init__(self, settings: Settings) -> None: foot_name="right", ) + self.add_periodicity_expression(all_contact_points) + def get_function_inputs_dict(self): sym = self.ocp.symbolic_structure function_inputs = { @@ -503,7 +505,9 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): ) problem.add_dynamics( hp.dot(sym.system.centroidal_momentum) == centroidal_dynamics, # noqa - x0=problem.initial(sym.initial_state.centroidal_momentum), # noqa + x0=problem.initial(sym.initial_state.centroidal_momentum) + if self.settings.periodicity_expression_type is hp.ExpressionType.skip + else None, # noqa dt=sym.dt, integrator=default_integrator, name="centroidal_momentum_dynamics", @@ -806,6 +810,41 @@ def add_contact_point_regularization( scaling=self.settings.contact_force_control_cost_multiplier, ) + def add_periodicity_expression(self, all_contact_points): + problem = self.ocp.problem + sym = self.ocp.symbolic_structure # type: Variables + initial_controls = [] + final_controls = [] + for point in all_contact_points: + initial_controls.append(problem.initial(point.u_v)) + initial_controls.append(problem.initial(point.f_dot)) + final_controls.append(problem.final(point.u_v)) + final_controls.append(problem.final(point.f_dot)) + initial_controls.append(problem.initial(sym.system.centroidal_momentum)) + final_controls.append(problem.final(sym.system.centroidal_momentum)) + initial_controls.append( + problem.initial(sym.system.kinematics.base.linear_velocity) + ) + initial_controls.append( + problem.initial(sym.system.kinematics.base.quaternion_velocity_xyzw) + ) + initial_controls.append( + problem.initial(sym.system.kinematics.joints.velocities) + ) + final_controls.append(problem.final(sym.system.kinematics.base.linear_velocity)) + final_controls.append( + problem.final(sym.system.kinematics.base.quaternion_velocity_xyzw) + ) + final_controls.append(problem.final(sym.system.kinematics.joints.velocities)) + problem.add_expression( + mode=self.settings.periodicity_expression_type, + expression=cs.MX( + cs.vertcat(*initial_controls) == cs.vertcat(*final_controls) + ), + name="periodicity_expression", + scaling=self.settings.periodicity_expression_weight, + ) + def set_initial_guess(self, initial_guess: Variables) -> None: self.ocp.problem.set_initial_guess(initial_guess) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index 76ab1032..4b139872 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -35,9 +35,11 @@ class Settings: minimum_joint_velocities: np.ndarray = dataclasses.field(default=None) final_state_expression_type: hp.ExpressionType = dataclasses.field(default=None) - final_state_expression_weight: float = dataclasses.field(default=None) + periodicity_expression_type: hp.ExpressionType = dataclasses.field(default=None) + periodicity_expression_weight: float = dataclasses.field(default=None) + contacts_centroid_cost_multiplier: float = dataclasses.field(default=None) com_linear_velocity_cost_weights: np.ndarray = dataclasses.field(default=None) @@ -88,6 +90,8 @@ def __post_init__(self) -> None: 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 def is_valid(self) -> bool: number_of_joints = len(self.joints_name_list) From 9bbf011b48cabff0f4813c320481a5a5ecb009dd Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 11:58:08 +0100 Subject: [PATCH 078/100] Removed force plotters in the periodic step file --- .../main_periodic_step.py | 42 ------------------- 1 file changed, 42 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 0c90da39..0cc46c4e 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -413,43 +413,6 @@ def get_references( print("Press [Enter] to visualize the solution.") input() - plotter_settings = hp_rp.FootContactStatePlotterSettings() - plotter_settings.terrain = planner_settings.terrain - left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) - right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) - - left_foot_plotter.plot_complementarity( - states=left_contact_points, - time_s=output.values.dt, - title="Left Foot Complementarity", - blocking=False, - ) - right_foot_plotter.plot_complementarity( - states=right_contact_points, - time_s=output.values.dt, - title="Right Foot Complementarity", - blocking=False, - ) - - left_foot_plotter.plot_forces( - states=left_contact_points, - time_s=output.values.dt, - title="Left Foot Forces", - blocking=False, - ) - right_foot_plotter.plot_forces( - states=right_contact_points, - time_s=output.values.dt, - title="Right Foot Forces", - blocking=False, - ) - - visualizer.visualize( - states=humanoid_states, - timestep_s=output.values.dt, - time_multiplier=2.0, - ) - visualizer.visualize( states=humanoid_states, timestep_s=output.values.dt, @@ -457,8 +420,3 @@ def get_references( save=True, file_name_stem="humanoid_walking_single_step", ) - - print("Press [Enter] to exit.") - input() - left_foot_plotter.close() - right_foot_plotter.close() From 9e26e2cbe4050bb41d429dafc7530996db406eed Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 11:59:34 +0100 Subject: [PATCH 079/100] Added possibility to move from HumanoidState to ExtendedhumanoidState --- .../humanoid_kinodynamic/variables.py | 34 +++++++++++++++++++ 1 file changed, 34 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index 096854af..e38149e2 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -1,4 +1,5 @@ import dataclasses +from typing import TypeVar import adam.casadi import numpy as np @@ -113,6 +114,9 @@ def __post_init__( self.joint_regularization = np.zeros((number_of_joints, 1)) +TExtendedContactPoint = TypeVar("TExtendedContactPoint", bound="ExtendedContactPoint") + + @dataclasses.dataclass class ExtendedContactPoint( hp_rp.ContactPointState, @@ -132,6 +136,18 @@ def to_contact_point_state(self) -> hp_rp.ContactPointState: output.descriptor = self.descriptor return output + @staticmethod + def from_contact_point_state( + input_state: hp_rp.ContactPointState, + ) -> TExtendedContactPoint: + output = ExtendedContactPoint(input_descriptor=input_state.descriptor) + output.p = input_state.p + output.f = input_state.f + output.u_v = None + output.f_dot = None + output.v = None + return output + @dataclasses.dataclass class FeetContactPointsExtended(hp.OptimizationObject): @@ -148,6 +164,16 @@ def to_feet_contact_points(self) -> hp_rp.FeetContactPoints: ) return output + def from_feet_contact_points(self, input_points: hp_rp.FeetContactPoints) -> None: + self.left = [ + ExtendedContactPoint.from_contact_point_state(point) + for point in input_points.left + ] + self.right = [ + ExtendedContactPoint.from_contact_point_state(point) + for point in input_points.right + ] + @dataclasses.dataclass class ExtendedHumanoid(hp.OptimizationObject): @@ -193,6 +219,14 @@ def to_humanoid_state(self) -> hp_rp.HumanoidState: output.com = self.com return output + def from_humanoid_state(self, input_state: hp_rp.HumanoidState) -> None: + self.contact_points.from_feet_contact_points(input_state.contact_points) + self.kinematics = hp_rp.FloatingBaseSystem.from_floating_base_system_state( + input_state.kinematics + ) + self.com = input_state.com + self.centroidal_momentum = None + @dataclasses.dataclass class ExtendedHumanoidState(hp_rp.HumanoidState): From e15397c4c80a39e434b186637697aa20095be858 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 11:10:35 +0100 Subject: [PATCH 080/100] Added __init__.py in humanoid pose finder --- src/hippopt/turnkey_planners/humanoid_pose_finder/__init__.py | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 src/hippopt/turnkey_planners/humanoid_pose_finder/__init__.py diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/__init__.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/__init__.py new file mode 100644 index 00000000..e69de29b From 5e768083c59fb2658d034b9ab0cddfb4a613790e Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 11:25:21 +0100 Subject: [PATCH 081/100] Specified private functions in pose finder --- .../humanoid_pose_finder/planner.py | 26 +++++++++---------- 1 file changed, 13 insertions(+), 13 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 7cd0e831..3918d273 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -220,7 +220,7 @@ def __init__(self, settings: Settings): variables = self.op.variables # type: Variables - function_inputs = self.get_function_inputs_dict() + function_inputs = self._get_function_inputs_dict() # Normalized quaternion computation normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( @@ -252,7 +252,7 @@ def __init__(self, settings: Settings): ) for i, point in enumerate(all_contact_points): - self.add_contact_point_feasibility( + self._add_contact_point_feasibility( relaxed_complementarity_fun, friction_margin_fun, height_fun, @@ -260,30 +260,30 @@ def __init__(self, settings: Settings): point, ) - self.add_contact_kinematic_consistency( + self._add_contact_kinematic_consistency( function_inputs, normalized_quaternion, point, point_kinematics_functions, ) - self.add_kinematics_constraints( + self._add_kinematics_constraints( function_inputs, normalized_quaternion, all_contact_points ) - self.add_kinematics_regularization( + self._add_kinematics_regularization( function_inputs=function_inputs, normalized_quaternion=normalized_quaternion ) - self.add_foot_regularization( + self._add_foot_regularization( points=variables.state.contact_points.left, references=variables.references.state.contact_points.left, ) - self.add_foot_regularization( + self._add_foot_regularization( points=variables.state.contact_points.right, references=variables.references.state.contact_points.right, ) - def get_function_inputs_dict(self): + def _get_function_inputs_dict(self): variables = self.op.variables function_inputs = { "mass_name": variables.mass.name(), @@ -310,7 +310,7 @@ def get_function_inputs_dict(self): } return function_inputs - def add_kinematics_constraints( + def _add_kinematics_constraints( self, function_inputs: dict, normalized_quaternion: cs.MX, @@ -380,7 +380,7 @@ def add_kinematics_constraints( name="joint_position_bounds", ) - def add_kinematics_regularization( + def _add_kinematics_regularization( self, function_inputs: dict, normalized_quaternion: cs.MX ): problem = self.op.problem @@ -442,7 +442,7 @@ def add_kinematics_regularization( scaling=self.settings.joint_regularization_cost_multiplier, ) - def add_contact_kinematic_consistency( + def _add_contact_kinematic_consistency( self, function_inputs: dict, normalized_quaternion: cs.MX, @@ -475,7 +475,7 @@ def add_contact_kinematic_consistency( name=point.p.name() + "_kinematics_consistency", ) - def add_contact_point_feasibility( + def _add_contact_point_feasibility( self, complementarity_margin_fun: cs.Function, friction_margin_fun: cs.Function, @@ -522,7 +522,7 @@ def add_contact_point_feasibility( name=point.f.name() + "_friction", ) - def add_foot_regularization( + def _add_foot_regularization( self, points: list[hp_rp.ContactPointState], references: list[hp_rp.ContactPointState], From c735383693f7ca2826ba66947683bca1e76b434d Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 11:34:05 +0100 Subject: [PATCH 082/100] Specified private functions in kinodynamic planner --- .../humanoid_kinodynamic/planner.py | 46 +++++++++---------- 1 file changed, 23 insertions(+), 23 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 7e6d4949..cd63cb71 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -50,7 +50,7 @@ def __init__(self, settings: Settings) -> None: sym = self.ocp.symbolic_structure # type: Variables - function_inputs = self.get_function_inputs_dict() + function_inputs = self._get_function_inputs_dict() # Normalized quaternion computation normalized_quaternion_fun = hp_rp.quaternion_xyzw_normalization( @@ -90,9 +90,9 @@ def __init__(self, settings: Settings) -> None: ) for i, point in enumerate(all_contact_points): - self.add_point_dynamics(point, initial_state=all_contact_initial_state[i]) + self._add_point_dynamics(point, initial_state=all_contact_initial_state[i]) - self.add_contact_point_feasibility( + self._add_contact_point_feasibility( dcc_margin_fun, dcc_planar_fun, friction_margin_fun, @@ -101,39 +101,39 @@ def __init__(self, settings: Settings) -> None: point, ) - self.add_contact_kinematic_consistency( + self._add_contact_kinematic_consistency( function_inputs, normalized_quaternion, point, point_kinematics_functions, ) - self.add_contact_point_regularization( + self._add_contact_point_regularization( point=point, desired_swing_height=sym.references.feet.desired_swing_height, function_inputs=function_inputs, ) - self.add_robot_dynamics(all_contact_points, function_inputs) + self._add_robot_dynamics(all_contact_points, function_inputs) - self.add_kinematics_constraints( + self._add_kinematics_constraints( function_inputs, height_fun, normalized_quaternion ) - self.add_kinematics_regularization( + self._add_kinematics_regularization( function_inputs=function_inputs, base_quaternion_normalized=normalized_quaternion, ) - self.add_contact_centroids_expressions(function_inputs) + self._add_contact_centroids_expressions(function_inputs) - self.add_foot_regularization( + self._add_foot_regularization( points=sym.system.contact_points.left, descriptors=self.settings.contact_points.left, references=sym.references.feet.left, function_inputs=function_inputs, foot_name="left", ) - self.add_foot_regularization( + self._add_foot_regularization( points=sym.system.contact_points.right, descriptors=self.settings.contact_points.right, references=sym.references.feet.right, @@ -141,9 +141,9 @@ def __init__(self, settings: Settings) -> None: foot_name="right", ) - self.add_periodicity_expression(all_contact_points) + self._add_periodicity_expression(all_contact_points) - def get_function_inputs_dict(self): + def _get_function_inputs_dict(self): sym = self.ocp.symbolic_structure function_inputs = { "mass_name": sym.mass.name(), @@ -178,7 +178,7 @@ def get_function_inputs_dict(self): } return function_inputs - def add_contact_centroids_expressions(self, function_inputs): + def _add_contact_centroids_expressions(self, function_inputs): problem = self.ocp.problem sym = self.ocp.symbolic_structure # type: Variables @@ -229,7 +229,7 @@ def get_centroid( scaling=self.settings.contacts_centroid_cost_multiplier, ) - def add_kinematics_constraints( + def _add_kinematics_constraints( self, function_inputs: dict, height_fun: cs.Function, @@ -362,7 +362,7 @@ def add_kinematics_constraints( scaling=self.settings.final_state_expression_weight, ) - def add_kinematics_regularization( + def _add_kinematics_regularization( self, function_inputs: dict, base_quaternion_normalized: cs.MX ): problem = self.ocp.problem @@ -447,7 +447,7 @@ def add_kinematics_regularization( scaling=self.settings.joint_regularization_cost_multiplier, ) - def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): + def _add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): problem = self.ocp.problem sym = self.ocp.symbolic_structure # type: Variables default_integrator = self.settings.integrator @@ -513,7 +513,7 @@ def add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): name="centroidal_momentum_dynamics", ) - def add_contact_kinematic_consistency( + def _add_contact_kinematic_consistency( self, function_inputs: dict, normalized_quaternion: cs.MX, @@ -547,7 +547,7 @@ def add_contact_kinematic_consistency( name=point.p.name() + "_kinematics_consistency", ) - def add_contact_point_feasibility( + def _add_contact_point_feasibility( self, dcc_margin_fun: cs.Function, dcc_planar_fun: cs.Function, @@ -634,7 +634,7 @@ def add_contact_point_feasibility( name=point.f_dot.name() + "_bounds", # noqa ) - def add_point_dynamics( + def _add_point_dynamics( self, point: ExtendedContactPoint, initial_state: hp_rp.ContactPointState ) -> None: default_integrator = self.settings.integrator @@ -659,7 +659,7 @@ def add_point_dynamics( name=point.p.name() + "_dynamics", ) - def add_foot_regularization( + def _add_foot_regularization( self, points: list[ExtendedContactPoint], descriptors: list[hp_rp.ContactPointDescriptor], @@ -768,7 +768,7 @@ def add_foot_regularization( scaling=self.settings.foot_yaw_regularization_cost_multiplier, ) - def add_contact_point_regularization( + def _add_contact_point_regularization( self, point: ExtendedContactPoint, desired_swing_height: hp.StorageType, @@ -810,7 +810,7 @@ def add_contact_point_regularization( scaling=self.settings.contact_force_control_cost_multiplier, ) - def add_periodicity_expression(self, all_contact_points): + def _add_periodicity_expression(self, all_contact_points): problem = self.ocp.problem sym = self.ocp.symbolic_structure # type: Variables initial_controls = [] From 9ad32ad8bdd91ee5f87ad3f3b44afca526f68738 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 12:45:01 +0100 Subject: [PATCH 083/100] Separated application of mass regularization in kinodynamic planner --- .../humanoid_kinodynamic/planner.py | 103 ++++++++++++++---- 1 file changed, 79 insertions(+), 24 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index cd63cb71..a7c9d163 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -845,14 +845,86 @@ def _add_periodicity_expression(self, all_contact_points): scaling=self.settings.periodicity_expression_weight, ) + def _apply_mass_regularization(self, input_var: Variables) -> Variables: + assert self.numeric_mass > 0 + output = input_var + if output.initial_state is not None: + if output.initial_state.centroidal_momentum is not None: + output.initial_state.centroidal_momentum /= self.numeric_mass + for point in ( + output.initial_state.contact_points.left + + output.initial_state.contact_points.right + ): + point.f /= self.numeric_mass + + if output.final_state is not None: + for point in ( + output.final_state.contact_points.left + + output.final_state.contact_points.right + ): + point.f /= self.numeric_mass + + if output.system is None: + return output + + system_list = ( + output.system if isinstance(output.system, list) else [output.system] + ) + + for system in system_list: + if system.centroidal_momentum is not None: + system.centroidal_momentum /= self.numeric_mass + for point in system.contact_points.left + system.contact_points.right: + point.f /= self.numeric_mass + + return output + + def _undo_mass_regularization(self, input_var: Variables) -> Variables: + assert self.numeric_mass > 0 + output = input_var + if output.initial_state is not None: + if output.initial_state.centroidal_momentum is not None: + output.initial_state.centroidal_momentum *= self.numeric_mass + for point in ( + output.initial_state.contact_points.left + + output.initial_state.contact_points.right + ): + point.f *= self.numeric_mass + + if output.final_state is not None: + for point in ( + output.final_state.contact_points.left + + output.final_state.contact_points.right + ): + point.f *= self.numeric_mass + + if output.system is None: + return output + + system_list = ( + output.system if isinstance(output.system, list) else [output.system] + ) + + for system in system_list: + if system.centroidal_momentum is not None: + system.centroidal_momentum *= self.numeric_mass + for point in system.contact_points.left + system.contact_points.right: + point.f *= self.numeric_mass + + return output + def set_initial_guess(self, initial_guess: Variables) -> None: - self.ocp.problem.set_initial_guess(initial_guess) + self.ocp.problem.set_initial_guess( + self._apply_mass_regularization(initial_guess) + ) def get_initial_guess(self) -> Variables: - return self.ocp.problem.get_initial_guess() + return self._undo_mass_regularization(self.ocp.problem.get_initial_guess()) def set_references(self, references: References | list[References]) -> None: - guess = self.get_initial_guess() + guess = ( + self.ocp.problem.get_initial_guess() + ) # Avoid the undo of the mass regularization assert isinstance(guess.references, list) assert not isinstance(references, list) or len(references) == len( @@ -862,36 +934,19 @@ def set_references(self, references: References | list[References]) -> None: guess.references[i] = ( references[i] if isinstance(references, list) else references ) - self.ocp.problem.set_initial_guess(guess) + self.ocp.problem.set_initial_guess(guess) # Avoid the mass regularization def set_initial_state(self, initial_state: ExtendedHumanoidState) -> None: guess = self.get_initial_guess() guess.initial_state = initial_state - guess.initial_state.centroidal_momentum /= self.numeric_mass - for point in ( - guess.initial_state.contact_points.left - + guess.initial_state.contact_points.right - ): - point.f /= self.numeric_mass - self.ocp.problem.set_initial_guess(guess) + self.set_initial_guess(guess) def set_final_state(self, final_state: hp_rp.HumanoidState) -> None: guess = self.get_initial_guess() guess.final_state = final_state - for point in ( - guess.final_state.contact_points.left - + guess.final_state.contact_points.right - ): - point.f /= self.numeric_mass - self.ocp.problem.set_initial_guess(guess) + self.set_initial_guess(guess) def solve(self) -> hp.Output[Variables]: output = self.ocp.problem.solve() - - values = output.values - - for s in values.system: - for point in s.contact_points.left + s.contact_points.right: - point.f *= values.mass - + output.values = self._undo_mass_regularization(output.values) return output From a9cb730f8140b662de8348eefb49241a3f1f63b5 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 12:45:40 +0100 Subject: [PATCH 084/100] Using a single function to compute initial and final state --- .../main_periodic_step.py | 106 ++++++------------ 1 file changed, 36 insertions(+), 70 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 0cc46c4e..0b6abacc 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -194,10 +194,13 @@ def get_visualizer_settings( return output_viz_settings -def compute_initial_state( +def compute_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, -) -> walking_variables.ExtendedHumanoidState: + 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, @@ -232,13 +235,7 @@ def compute_initial_state( number_of_joints=len(desired_joints), ) - pf_ref.state.com = np.array([0.15, 0.0, 0.7]) - desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() - ) - desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() - ) + pf_ref.state.com = desired_com_position pf_ref.state.contact_points.left = ( hp_rp.FootContactState.from_parent_frame_transform( descriptor=input_settings.contact_points.left, @@ -263,11 +260,32 @@ def compute_initial_state( 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, +) -> walking_variables.ExtendedHumanoidState: + desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ) + desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + ) + + output_pf = compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=np.array([0.15, 0.0, 0.7]), + 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.values.state.contact_points - output_state.kinematics = output_pf.values.state.kinematics - output_state.com = output_pf.values.state.com + 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)) @@ -278,73 +296,21 @@ def compute_final_state( input_settings: walking_settings.Settings, pf_input: pose_finder.Planner, ) -> hp_rp.HumanoidState: - desired_joints = np.deg2rad( - [ - 7, - 0.12, - -0.01, - 12.0, - 7.0, - -12.0, - 40.769, - 12.0, - 7.0, - -12.0, - 40.769, - 5.76, - 1.61, - -0.31, - -31.64, - -20.52, - -1.52, - 5.76, - 1.61, - -0.31, - -31.64, - -20.52, - -1.52, - ] - ) - assert len(input_settings.joints_name_list) == len(desired_joints) - - pf_ref = pose_finder.References( - contact_point_descriptors=pf_settings.contact_points, - number_of_joints=len(desired_joints), - ) - - pf_ref.state.com = np.array([0.75, 0.0, 0.7]) desired_left_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.60, 0.1, 0.0]), liecasadi.SO3.Identity() ) desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( np.array([0.90, -0.1, 0.0]), liecasadi.SO3.Identity() ) - pf_ref.state.contact_points.left = ( - hp_rp.FootContactState.from_parent_frame_transform( - descriptor=input_settings.contact_points.left, - transform=desired_left_foot_pose, - ) - ) - pf_ref.state.contact_points.right = ( - hp_rp.FootContactState.from_parent_frame_transform( - descriptor=input_settings.contact_points.right, - transform=desired_right_foot_pose, - ) - ) - pf_ref.state.kinematics.base.quaternion_xyzw = ( - liecasadi.SO3.Identity().as_quat().coeffs() + return compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=np.array([0.75, 0.0, 0.7]), + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, ) - pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() - - pf_ref.state.kinematics.joints.positions = desired_joints - - pf_input.set_references(pf_ref) - - output_pf = pf_input.solve() - return output_pf.values.state - def get_references( input_settings: walking_settings.Settings, From f042851344cf892d89834d06e7aee288bd4bfc28 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 16:44:58 +0100 Subject: [PATCH 085/100] Improved the check on the existence of centroidal_momentum when applying mass regularization --- .../turnkey_planners/humanoid_kinodynamic/planner.py | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index a7c9d163..218a523e 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -849,7 +849,11 @@ def _apply_mass_regularization(self, input_var: Variables) -> Variables: assert self.numeric_mass > 0 output = input_var if output.initial_state is not None: - if output.initial_state.centroidal_momentum is not None: + if ( + output.initial_state.centroidal_momentum is not None + and len(output.initial_state.centroidal_momentum.shape) > 0 + and output.initial_state.centroidal_momentum.shape[0] == 6 + ): output.initial_state.centroidal_momentum /= self.numeric_mass for point in ( output.initial_state.contact_points.left @@ -883,7 +887,11 @@ def _undo_mass_regularization(self, input_var: Variables) -> Variables: assert self.numeric_mass > 0 output = input_var if output.initial_state is not None: - if output.initial_state.centroidal_momentum is not None: + if ( + output.initial_state.centroidal_momentum is not None + and len(output.initial_state.centroidal_momentum.shape) > 0 + and output.initial_state.centroidal_momentum.shape[0] == 6 + ): output.initial_state.centroidal_momentum *= self.numeric_mass for point in ( output.initial_state.contact_points.left From 838921e68c92e8f892df57987eef4868fbac4f95 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 16:45:31 +0100 Subject: [PATCH 086/100] The function to convert from HumanoidState to ExtendedHumanoid is now static --- .../humanoid_kinodynamic/variables.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index e38149e2..54ba3330 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -175,6 +175,9 @@ def from_feet_contact_points(self, input_points: hp_rp.FeetContactPoints) -> Non ] +TExtendedHumanoid = TypeVar("TExtendedHumanoid", bound="ExtendedHumanoid") + + @dataclasses.dataclass class ExtendedHumanoid(hp.OptimizationObject): contact_points: hp.CompositeType[ @@ -219,13 +222,16 @@ def to_humanoid_state(self) -> hp_rp.HumanoidState: output.com = self.com return output - def from_humanoid_state(self, input_state: hp_rp.HumanoidState) -> None: - self.contact_points.from_feet_contact_points(input_state.contact_points) - self.kinematics = hp_rp.FloatingBaseSystem.from_floating_base_system_state( + @staticmethod + def from_humanoid_state(input_state: hp_rp.HumanoidState) -> TExtendedHumanoid: + output = ExtendedHumanoid() + output.contact_points.from_feet_contact_points(input_state.contact_points) + output.kinematics = hp_rp.FloatingBaseSystem.from_floating_base_system_state( input_state.kinematics ) - self.com = input_state.com - self.centroidal_momentum = None + output.com = input_state.com + output.centroidal_momentum = None + return output @dataclasses.dataclass From 76d73a692bb00b1b8ebafd341bef209555c84ca4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 16:46:21 +0100 Subject: [PATCH 087/100] Specifying a full guess in the periodic step --- .../main_periodic_step.py | 155 +++++++++++++++--- 1 file changed, 131 insertions(+), 24 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 0b6abacc..c078cdb0 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -266,18 +266,17 @@ def compute_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 = liecasadi.SE3.from_translation_and_rotation( - np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() - ) - desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() - ) - + 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 output_pf = compute_state( input_settings=input_settings, pf_input=pf_input, - desired_com_position=np.array([0.15, 0.0, 0.7]), + desired_com_position=desired_com_position, desired_left_foot_pose=desired_left_foot_pose, desired_right_foot_pose=desired_right_foot_pose, ) @@ -292,21 +291,41 @@ def compute_initial_state( return output_state -def compute_final_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 = liecasadi.SE3.from_translation_and_rotation( - np.array([0.60, 0.1, 0.0]), liecasadi.SO3.Identity() - ) - desired_right_foot_pose = liecasadi.SE3.from_translation_and_rotation( - np.array([0.90, -0.1, 0.0]), liecasadi.SO3.Identity() + 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=np.array([0.75, 0.0, 0.7]), + desired_com_position=desired_com_position, desired_left_foot_pose=desired_left_foot_pose, desired_right_foot_pose=desired_right_foot_pose, ) @@ -339,27 +358,109 @@ def get_references( pf_settings = get_pose_finder_settings(input_settings=planner_settings) pf = pose_finder.Planner(settings=pf_settings) - visualizer_settings = get_visualizer_settings(input_settings=planner_settings) - visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + horizon = planner_settings.horizon_length * planner_settings.time_step + + contact_phases_guess = hp_rp.FeetContactPhasesDescriptor() + contact_phases_guess.left = [ + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.3, 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([0.6, 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([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.60, -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([0.9, -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, ) - visualizer.visualize(initial_state) - - planner.set_initial_state(initial_state) 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)) - print("Press [Enter] to visualize the final desired state.") + 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(final_state) + visualizer.visualize( + states=guess, + timestep_s=planner_settings.time_step, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_periodic_guess", + ) print("Starting the planner...") @@ -369,7 +470,13 @@ def get_references( ) planner.set_references(references) - planner.set_final_state(final_state) + 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() @@ -384,5 +491,5 @@ def get_references( timestep_s=output.values.dt, time_multiplier=1.0, save=True, - file_name_stem="humanoid_walking_single_step", + file_name_stem="humanoid_walking_periodic", ) From 09f7390a6b32a8a8af087d4a50bb570eeb6b41c3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 17:20:58 +0100 Subject: [PATCH 088/100] Specify the step length in the periodic step main --- .../humanoid_kinodynamic/main_periodic_step.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index c078cdb0..61180178 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -360,6 +360,8 @@ def get_references( 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( @@ -367,7 +369,7 @@ def get_references( np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() ), mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.3, 0.1, 0.05]), liecasadi.SO3.Identity() + np.array([step_length / 2, 0.1, 0.05]), liecasadi.SO3.Identity() ), force=np.array([0, 0, 100.0]), activation_time=None, @@ -375,7 +377,7 @@ def get_references( ), hp_rp.FootContactPhaseDescriptor( transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.6, 0.1, 0.0]), liecasadi.SO3.Identity() + np.array([step_length, 0.1, 0.0]), liecasadi.SO3.Identity() ), mid_swing_transform=None, force=np.array([0, 0, 100.0]), @@ -387,10 +389,10 @@ def get_references( contact_phases_guess.right = [ hp_rp.FootContactPhaseDescriptor( transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.3, -0.1, 0.0]), liecasadi.SO3.Identity() + np.array([step_length / 2, -0.1, 0.0]), liecasadi.SO3.Identity() ), mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.60, -0.1, 0.05]), liecasadi.SO3.Identity() + np.array([step_length, -0.1, 0.05]), liecasadi.SO3.Identity() ), force=np.array([0, 0, 100.0]), activation_time=None, @@ -398,7 +400,7 @@ def get_references( ), hp_rp.FootContactPhaseDescriptor( transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.9, -0.1, 0.0]), liecasadi.SO3.Identity() + np.array([1.5 * step_length, -0.1, 0.0]), liecasadi.SO3.Identity() ), mid_swing_transform=None, force=np.array([0, 0, 100.0]), From 9e1e001d920d35c17b88c3a414ddcbf6e98c2ab6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 6 Dec 2023 14:27:30 +0900 Subject: [PATCH 089/100] Specified the reference joint trajectory --- .../main_periodic_step.py | 38 +++++++++++-------- 1 file changed, 22 insertions(+), 16 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 61180178..f15c6999 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -100,10 +100,10 @@ def get_planner_settings() -> walking_settings.Settings: settings.com_linear_velocity_cost_weights = [10.0, 0.1, 1.0] settings.com_linear_velocity_cost_multiplier = 1.0 settings.desired_frame_quaternion_cost_frame_name = "chest" - settings.desired_frame_quaternion_cost_multiplier = 90.0 + settings.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 = 0.1 + 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 @@ -123,7 +123,7 @@ def get_planner_settings() -> walking_settings.Settings: "dual_inf_tol": 1000.0, "compl_inf_tol": 1e-2, "constr_viol_tol": 1e-4, - "acceptable_tol": 1e0, + "acceptable_tol": 5e-1, "acceptable_iter": 2, "acceptable_compl_inf_tol": 1.0, "warm_start_bound_frac": 1e-2, @@ -333,20 +333,26 @@ def compute_final_state( def get_references( input_settings: walking_settings.Settings, - desired_state: hp_rp.HumanoidState, -) -> walking_variables.References: - output_reference = walking_variables.References( - number_of_joints=len(input_settings.joints_name_list), - number_of_points_left=len(input_settings.contact_points.left), - number_of_points_right=len(input_settings.contact_points.right), - ) + 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_state.kinematics.joints.positions - output_reference.com_linear_velocity = [0.1, 0.0, 0.0] + 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_reference + return output_list if __name__ == "__main__": @@ -468,7 +474,7 @@ def get_references( references = get_references( input_settings=planner_settings, - desired_state=final_state, + desired_states=guess, ) planner.set_references(references) From 74e6a99525878480e86795ca2d6125ec617d3c8b Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 14:54:48 +0100 Subject: [PATCH 090/100] Fixed small typo in main_periodic_step.py --- .../turnkey_planners/humanoid_kinodynamic/main_periodic_step.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index f15c6999..04563a8e 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -16,7 +16,7 @@ def get_planner_settings() -> walking_settings.Settings: parser = argparse.ArgumentParser( - description="Trajectory Optimization of a forward walking motion on ergoCub.", + description="Trajectory Optimization of periodic walking motion on ergoCub.", ) parser.add_argument( "--urdf", From 6600e31e1dfcc001e51c4ef0dee290d4e9d4e089 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 5 Jan 2024 12:22:31 +0100 Subject: [PATCH 091/100] Added first version of file for walking on two steps stairs --- .../main_walking_on_stairs.py | 610 ++++++++++++++++++ 1 file changed, 610 insertions(+) create mode 100644 src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py new file mode 100644 index 00000000..86fe1c23 --- /dev/null +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -0,0 +1,610 @@ +import argparse +import logging + +import casadi as cs +import idyntree.bindings as idyntree +import liecasadi +import numpy as np + +import hippopt +import hippopt.robot_planning as hp_rp +import hippopt.turnkey_planners.humanoid_kinodynamic.planner as walking_planner +import hippopt.turnkey_planners.humanoid_kinodynamic.settings as walking_settings +import hippopt.turnkey_planners.humanoid_kinodynamic.variables as walking_variables +import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder + + +def get_terrain(length: float, width: float, height: float) -> hp_rp.TerrainDescriptor: + first_step_origin = np.array([1.5 * length, 0.0, 0.0]) + second_step_origin = np.array([2 * length, 0.0, 0.0]) + + terrain = hp_rp.SmoothTerrain.step( + length=2 * length, width=width, height=height, position=first_step_origin + ) + hp_rp.SmoothTerrain.step( + length=0.9 * length, width=width, height=height, position=second_step_origin + ) + terrain.set_name(f"two_steps_{length}_{width}_{height}") + return terrain + + +def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.Settings: + parser = argparse.ArgumentParser( + description="Trajectory Optimization of a walking motion" + " on ergoCub over stairs.", + ) + parser.add_argument( + "--urdf", + type=str, + required=True, + help="Path to the ergoCubGazeboV1_minContacts URDF file.", + ) + settings = walking_settings.Settings() + settings.terrain = terrain + settings.robot_urdf = parser.parse_args().urdf + 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 = 2.0 + settings.dcc_epsilon = 0.5 + settings.static_friction = 1.0 + 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": 10000, + "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, + "warm_start_bound_frac": 1e-2, + "warm_start_bound_push": 1e-2, + "warm_start_mult_bound_push": 1e-2, + "warm_start_slack_bound_frac": 1e-2, + "warm_start_slack_bound_push": 1e-2, + "warm_start_init_point": "yes", + "required_infeasibility_reduction": 0.8, + "perturb_dec_fact": 0.1, + "max_hessian_perturbation": 100.0, + } + + return settings + + +def get_pose_finder_settings( + input_settings: walking_settings.Settings, +) -> pose_finder.Settings: + number_of_joints = len(input_settings.joints_name_list) + settings = pose_finder.Settings() + settings.terrain = input_settings.terrain + settings.robot_urdf = input_settings.robot_urdf + settings.joints_name_list = input_settings.joints_name_list + + settings.root_link = input_settings.root_link + settings.desired_frame_quaternion_cost_frame_name = ( + input_settings.desired_frame_quaternion_cost_frame_name + ) + + settings.contact_points = input_settings.contact_points + + settings.relaxed_complementarity_epsilon = 1.0 + settings.static_friction = 0.3 + + settings.maximum_joint_positions = input_settings.maximum_joint_positions + settings.minimum_joint_positions = input_settings.minimum_joint_positions + + settings.joint_regularization_cost_weights = np.ones(number_of_joints) + settings.joint_regularization_cost_weights[:3] = 0.1 # torso + settings.joint_regularization_cost_weights[3:11] = 10.0 # arms + settings.joint_regularization_cost_weights[11:] = 1.0 # legs + + settings.base_quaternion_cost_multiplier = 50.0 + settings.desired_frame_quaternion_cost_multiplier = 100.0 + settings.joint_regularization_cost_multiplier = 0.1 + settings.force_regularization_cost_multiplier = 0.2 + settings.com_regularization_cost_multiplier = 10.0 + settings.average_force_regularization_cost_multiplier = 10.0 + settings.point_position_regularization_cost_multiplier = 100.0 + settings.casadi_function_options = input_settings.casadi_function_options + settings.casadi_opti_options = input_settings.casadi_opti_options + settings.casadi_solver_options = {} + + return settings + + +def get_visualizer_settings( + input_settings: walking_settings.Settings, +) -> hp_rp.HumanoidStateVisualizerSettings: + output_viz_settings = hp_rp.HumanoidStateVisualizerSettings() + output_viz_settings.robot_model = input_settings.robot_urdf + output_viz_settings.considered_joints = input_settings.joints_name_list + output_viz_settings.contact_points = input_settings.contact_points + output_viz_settings.terrain = input_settings.terrain + output_viz_settings.overwrite_terrain_files = True + output_viz_settings.working_folder = "./" + + return output_viz_settings + + +def compute_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + desired_com_position: np.ndarray, + desired_left_foot_pose: liecasadi.SE3, + desired_right_foot_pose: liecasadi.SE3, +) -> hp_rp.HumanoidState: + desired_joints = np.deg2rad( + [ + 7, + 0.12, + -0.01, + 12.0, + 7.0, + -12.0, + 40.769, + 12.0, + 7.0, + -12.0, + 40.769, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + 5.76, + 1.61, + -0.31, + -31.64, + -20.52, + -1.52, + ] + ) + assert len(input_settings.joints_name_list) == len(desired_joints) + + pf_ref = pose_finder.References( + contact_point_descriptors=pf_settings.contact_points, + number_of_joints=len(desired_joints), + ) + + pf_ref.state.com = desired_com_position + pf_ref.state.contact_points.left = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.left, + transform=desired_left_foot_pose, + ) + ) + pf_ref.state.contact_points.right = ( + hp_rp.FootContactState.from_parent_frame_transform( + descriptor=input_settings.contact_points.right, + transform=desired_right_foot_pose, + ) + ) + + pf_ref.state.kinematics.base.quaternion_xyzw = ( + liecasadi.SO3.Identity().as_quat().coeffs() + ) + + pf_ref.frame_quaternion_xyzw = liecasadi.SO3.Identity().as_quat().coeffs() + + pf_ref.state.kinematics.joints.positions = desired_joints + + pf_input.set_references(pf_ref) + initial_guess = pf_input.get_initial_guess() + initial_guess.state = pf_ref.state + initial_guess.state.kinematics.base.position = initial_guess.state.com + pf_input.set_initial_guess(initial_guess) + + output_pf = pf_input.solve() + return output_pf.values.state + + +def compute_initial_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> walking_variables.ExtendedHumanoidState: + desired_left_foot_pose = contact_guess.left[0].transform + desired_right_foot_pose = contact_guess.right[0].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] += 0.7 + output_pf = compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + ) + + output_state = walking_variables.ExtendedHumanoidState() + output_state.contact_points = output_pf.contact_points + output_state.kinematics = output_pf.kinematics + output_state.com = output_pf.com + + output_state.centroidal_momentum = np.zeros((6, 1)) + + return output_state + + +def compute_second_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[0].transform + desired_right_foot_pose = contact_guess.right[1].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] += 0.7 + return compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + ) + + +def compute_third_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[1].transform + desired_right_foot_pose = contact_guess.right[1].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] += 0.7 + return compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + ) + + +def compute_final_state( + input_settings: walking_settings.Settings, + pf_input: pose_finder.Planner, + contact_guess: hp_rp.FeetContactPhasesDescriptor, +) -> hp_rp.HumanoidState: + desired_left_foot_pose = contact_guess.left[1].transform + desired_right_foot_pose = contact_guess.right[2].transform + desired_com_position = ( + desired_left_foot_pose.translation() + desired_right_foot_pose.translation() + ) / 2.0 + desired_com_position[2] += 0.7 + return compute_state( + input_settings=input_settings, + pf_input=pf_input, + desired_com_position=desired_com_position, + desired_left_foot_pose=desired_left_foot_pose, + desired_right_foot_pose=desired_right_foot_pose, + ) + + +def get_references( + input_settings: walking_settings.Settings, + desired_states: list[hp_rp.HumanoidState], +) -> list[walking_variables.References]: + output_list = [] + + for i in range(input_settings.horizon_length): + output_reference = walking_variables.References( + number_of_joints=len(input_settings.joints_name_list), + number_of_points_left=len(input_settings.contact_points.left), + number_of_points_right=len(input_settings.contact_points.right), + ) + + output_reference.contacts_centroid_cost_weights = [100, 100, 10] + output_reference.contacts_centroid = [0.3, 0.0, 0.0] + output_reference.joint_regularization = desired_states[ + i + ].kinematics.joints.positions + output_reference.com_linear_velocity = [0.1, 0.0, 0.0] + output_list.append(output_reference) + + return output_list + + +if __name__ == "__main__": + logging.basicConfig(level=logging.INFO) + + step_length = 0.7 + step_height = 0.1 + + planner_settings = get_planner_settings( + get_terrain(length=step_length / 2, width=0.8, height=step_height) + ) + planner = walking_planner.Planner(settings=planner_settings) + + pf_settings = get_pose_finder_settings(input_settings=planner_settings) + pf = pose_finder.Planner(settings=pf_settings) + + horizon = planner_settings.horizon_length * planner_settings.time_step + + contact_phases_guess = hp_rp.FeetContactPhasesDescriptor() + contact_phases_guess.left = [ + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, 0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length / 2, 0.1, 0.05 + 2 * step_height]), + liecasadi.SO3.Identity(), + ), + force=np.array([0, 0, 100.0]), + activation_time=None, + deactivation_time=horizon * 3.0 / 7.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length, 0.1, 2 * step_height]), + liecasadi.SO3.Identity(), + ), + mid_swing_transform=None, + force=np.array([0, 0, 100.0]), + activation_time=horizon * 4.0 / 7.0, + deactivation_time=None, + ), + ] + + contact_phases_guess.right = [ + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.25 * step_length, -0.1, 0.05 + step_height]), + liecasadi.SO3.Identity(), + ), + force=np.array([0, 0, 100.0]), + activation_time=None, + deactivation_time=horizon / 7.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.5 * step_length, -0.1, step_height]), + liecasadi.SO3.Identity(), + ), + mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( + np.array([0.75 * step_length, -0.1, 0.05 + 2 * step_height]), + liecasadi.SO3.Identity(), + ), + force=np.array([0, 0, 100.0]), + activation_time=horizon * 2.0 / 7.0, + deactivation_time=horizon * 5.0 / 7.0, + ), + hp_rp.FootContactPhaseDescriptor( + transform=liecasadi.SE3.from_translation_and_rotation( + np.array([step_length, -0.1, 2 * step_height]), + liecasadi.SO3.Identity(), + ), + mid_swing_transform=None, + force=np.array([0, 0, 100.0]), + activation_time=horizon * 6.0 / 7.0, + deactivation_time=None, + ), + ] + + visualizer_settings = get_visualizer_settings(input_settings=planner_settings) + visualizer = hp_rp.HumanoidStateVisualizer(settings=visualizer_settings) + + initial_state = compute_initial_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + + # print("Press [Enter] to visualize the initial state.") + # input() + # + # visualizer.visualize(initial_state) + + 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)) + + # print("Press [Enter] to visualize the final desired state.") + # input() + # + # visualizer.visualize(final_state) + + second_state = compute_second_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + + # print("Press [Enter] to visualize the first intermediate state.") + # input() + # + # visualizer.visualize(second_state) + + third_state = compute_third_state( + input_settings=planner_settings, + pf_input=pf, + contact_guess=contact_phases_guess, + ) + + # print("Press [Enter] to visualize the second intermediate state.") + # input() + # + # visualizer.visualize(third_state) + + # Until the middle of the second double support phase + first_part_guess_length = (planner_settings.horizon_length * 5) // 14 + first_part_guess = hp_rp.humanoid_state_interpolator( + initial_state=initial_state, + final_state=second_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=first_part_guess_length, + dt=planner_settings.time_step, + ) + + # Until the middle of the third double support phase + second_part_guess_length = (planner_settings.horizon_length * 2) // 7 + second_part_guess = hp_rp.humanoid_state_interpolator( + initial_state=second_state, + final_state=third_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=second_part_guess_length, + dt=planner_settings.time_step, + t0=first_part_guess_length * planner_settings.time_step, + ) + + # Remaining part + third_part_guess_length = ( + planner_settings.horizon_length + - first_part_guess_length + - second_part_guess_length + ) + third_part_guess = hp_rp.humanoid_state_interpolator( + initial_state=third_state, + final_state=final_state, + contact_phases=contact_phases_guess, + contact_descriptor=planner_settings.contact_points, + number_of_points=third_part_guess_length, + dt=planner_settings.time_step, + t0=(first_part_guess_length + second_part_guess_length) + * planner_settings.time_step, + ) + + guess = first_part_guess + second_part_guess + third_part_guess + + # print("Press [Enter] to visualize the initial guess.") + # input() + # + # visualizer.visualize( + # states=guess, + # timestep_s=planner_settings.time_step, + # time_multiplier=1.0, + # save=True, + # file_name_stem="humanoid_walking_step_guess", + # ) + + print("Starting the planner...") + + references = get_references( + input_settings=planner_settings, + desired_states=guess, + ) + + planner.set_references(references) + planner_guess = planner.get_initial_guess() + planner_guess.system = [ + walking_variables.ExtendedHumanoid.from_humanoid_state(s) for s in guess + ] + planner_guess.initial_state = initial_state + planner_guess.final_state = final_state + planner.set_initial_guess(planner_guess) + + output = planner.solve() + + humanoid_states = [s.to_humanoid_state() for s in output.values.system] + left_contact_points = [s.contact_points.left for s in humanoid_states] + right_contact_points = [s.contact_points.right for s in humanoid_states] + print("Press [Enter] to visualize the solution.") + input() + + visualizer.visualize( + states=humanoid_states, + timestep_s=output.values.dt, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_step", + ) From 41bf0da0833b94c4706c50e7e05a90adff733d9b Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 5 Jan 2024 15:36:15 +0100 Subject: [PATCH 092/100] Intermediate acceptable results of walking on stairs --- .../main_walking_on_stairs.py | 34 +++++++++---------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py index 86fe1c23..a8ae1185 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -89,8 +89,8 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S top_left_point_position=np.array([0.116, 0.05, 0.0]), ) settings.planar_dcc_height_multiplier = 10.0 - settings.dcc_gain = 2.0 - settings.dcc_epsilon = 0.5 + settings.dcc_gain = 20.0 + settings.dcc_epsilon = 0.05 settings.static_friction = 1.0 settings.maximum_velocity_control = [2.0, 2.0, 5.0] settings.maximum_force_derivative = [500.0, 500.0, 500.0] @@ -118,14 +118,13 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S 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.joint_regularization_cost_multiplier = 1.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 = { @@ -401,6 +400,7 @@ def get_references( step_length = 0.7 step_height = 0.1 + swing_height = 0.1 planner_settings = get_planner_settings( get_terrain(length=step_length / 2, width=0.8, height=step_height) @@ -419,7 +419,7 @@ def get_references( 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 + 2 * step_height]), + np.array([step_length / 2, 0.1, swing_height + 2 * step_height]), liecasadi.SO3.Identity(), ), force=np.array([0, 0, 100.0]), @@ -444,7 +444,7 @@ def get_references( np.array([0.0, -0.1, 0.0]), liecasadi.SO3.Identity() ), mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.25 * step_length, -0.1, 0.05 + step_height]), + np.array([0.25 * step_length, -0.1, swing_height + step_height]), liecasadi.SO3.Identity(), ), force=np.array([0, 0, 100.0]), @@ -457,7 +457,7 @@ def get_references( liecasadi.SO3.Identity(), ), mid_swing_transform=liecasadi.SE3.from_translation_and_rotation( - np.array([0.75 * step_length, -0.1, 0.05 + 2 * step_height]), + np.array([0.75 * step_length, -0.1, swing_height + 2 * step_height]), liecasadi.SO3.Identity(), ), force=np.array([0, 0, 100.0]), @@ -566,16 +566,16 @@ def get_references( guess = first_part_guess + second_part_guess + third_part_guess - # print("Press [Enter] to visualize the initial guess.") - # input() - # - # visualizer.visualize( - # states=guess, - # timestep_s=planner_settings.time_step, - # time_multiplier=1.0, - # save=True, - # file_name_stem="humanoid_walking_step_guess", - # ) + print("Press [Enter] to visualize the initial guess.") + input() + + visualizer.visualize( + states=guess, + timestep_s=planner_settings.time_step, + time_multiplier=1.0, + save=True, + file_name_stem="humanoid_walking_step_guess", + ) print("Starting the planner...") From 7cc1cbfe780516497489a01fc25591d5ebddadf1 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 5 Jan 2024 17:20:25 +0100 Subject: [PATCH 093/100] Some further improvements on the step over stairs --- .../humanoid_kinodynamic/main_walking_on_stairs.py | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py index a8ae1185..a09759ff 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -73,7 +73,7 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S ) idyntree_model = idyntree_model_loader.model() settings.root_link = "root_link" - settings.horizon_length = 30 + settings.horizon_length = 50 settings.time_step = 0.1 settings.contact_points = hp_rp.FeetContactPointDescriptors() settings.contact_points.left = hp_rp.ContactPointDescriptor.rectangular_foot( @@ -89,8 +89,8 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S top_left_point_position=np.array([0.116, 0.05, 0.0]), ) settings.planar_dcc_height_multiplier = 10.0 - settings.dcc_gain = 20.0 - settings.dcc_epsilon = 0.05 + settings.dcc_gain = 40.0 + settings.dcc_epsilon = 0.01 settings.static_friction = 1.0 settings.maximum_velocity_control = [2.0, 2.0, 5.0] settings.maximum_force_derivative = [500.0, 500.0, 500.0] @@ -119,7 +119,7 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S settings.base_quaternion_cost_multiplier = 50.0 settings.base_quaternion_velocity_cost_multiplier = 0.001 settings.joint_regularization_cost_multiplier = 1.0 - settings.force_regularization_cost_multiplier = 10.0 + settings.force_regularization_cost_multiplier = 100.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 @@ -130,7 +130,7 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S settings.casadi_solver_options = { "max_iter": 10000, "linear_solver": "mumps", - "alpha_for_y": "dual-and-full", + # "alpha_for_y": "dual-and-full", "fast_step_computation": "yes", "hessian_approximation": "limited-memory", "tol": 1e-3, @@ -398,7 +398,7 @@ def get_references( if __name__ == "__main__": logging.basicConfig(level=logging.INFO) - step_length = 0.7 + step_length = 0.9 step_height = 0.1 swing_height = 0.1 From 9b831d69787ae352eeb7059c19f0517e04606600 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 9 Jan 2024 14:39:35 +0100 Subject: [PATCH 094/100] Allow using the opti callback in the kinodynamic planner --- .../turnkey_planners/humanoid_kinodynamic/planner.py | 10 ++++++++++ .../humanoid_kinodynamic/settings.py | 12 ++++++++++++ 2 files changed, 22 insertions(+) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 218a523e..43a36ba7 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -34,11 +34,21 @@ def __init__(self, settings: Settings) -> None: settings=self.settings, kin_dyn_object=self.kin_dyn_object ) + opti_callback = None + if self.settings.use_opti_callback: + opti_callback = ( + hp.opti_callback.BestCost() + & hp.opti_callback.AcceptablePrimalInfeasibility( + self.settings.acceptable_constraint_violation + ) + ) + optimization_solver = hp.OptiSolver( inner_solver=self.settings.opti_solver, problem_type=self.settings.problem_type, options_solver=self.settings.casadi_solver_options, options_plugin=self.settings.casadi_opti_options, + callback_criterion=opti_callback, ) ocp_solver = hp.MultipleShootingSolver(optimization_solver=optimization_solver) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py index 4b139872..51d15413 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/settings.py @@ -70,6 +70,14 @@ class Settings: problem_type: str = dataclasses.field(default="nlp") + use_opti_callback: bool = dataclasses.field(default=None) + + acceptable_constraint_violation: float = dataclasses.field(default=None) + + opti_callback_save_costs: bool = dataclasses.field(default=None) + + opti_callback_save_constraint_multipliers: bool = dataclasses.field(default=None) + casadi_function_options: dict = dataclasses.field(default_factory=dict) casadi_opti_options: dict = dataclasses.field(default_factory=dict) @@ -92,6 +100,10 @@ def __post_init__(self) -> None: self.final_state_expression_weight = 1.0 self.periodicity_expression_type = hp.ExpressionType.skip self.periodicity_expression_weight = 1.0 + self.use_opti_callback = False + self.acceptable_constraint_violation = 1e-3 + self.opti_callback_save_costs = True + self.opti_callback_save_constraint_multipliers = True def is_valid(self) -> bool: number_of_joints = len(self.joints_name_list) From 698f0aa5dede80a3c79584ed01bfa98244a4b974 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 9 Jan 2024 14:40:35 +0100 Subject: [PATCH 095/100] Use the opti callback in the walking over steps. Increase the constraint on the maximum feet height difference --- .../humanoid_kinodynamic/main_walking_on_stairs.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py index a09759ff..038cf389 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -97,7 +97,7 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S 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_feet_relative_height = 0.5 settings.maximum_joint_positions = cs.inf * np.ones(number_of_joints) settings.minimum_joint_positions = -cs.inf * np.ones(number_of_joints) for i in range(number_of_joints): @@ -113,7 +113,7 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S 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.com_linear_velocity_cost_multiplier = 0.0 settings.desired_frame_quaternion_cost_frame_name = "chest" settings.desired_frame_quaternion_cost_multiplier = 200.0 settings.base_quaternion_cost_multiplier = 50.0 @@ -121,10 +121,13 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S settings.joint_regularization_cost_multiplier = 1.0 settings.force_regularization_cost_multiplier = 100.0 settings.foot_yaw_regularization_cost_multiplier = 2000.0 - settings.swing_foot_height_cost_multiplier = 1000.0 + settings.swing_foot_height_cost_multiplier = 10.0 settings.contact_velocity_control_cost_multiplier = 5.0 settings.contact_force_control_cost_multiplier = 0.0001 settings.final_state_expression_type = hippopt.ExpressionType.subject_to + settings.use_opti_callback = True + settings.opti_callback_save_costs = False + settings.opti_callback_save_constraint_multipliers = False settings.casadi_function_options = {"cse": True} settings.casadi_opti_options = {"expand": True, "detect_simple_bounds": True} settings.casadi_solver_options = { @@ -400,7 +403,7 @@ def get_references( step_length = 0.9 step_height = 0.1 - swing_height = 0.1 + swing_height = 1.0 planner_settings = get_planner_settings( get_terrain(length=step_length / 2, width=0.8, height=step_height) From 761cfa1f8eef8c248fb5acec78a60a4e3d4c389c Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 11 Jan 2024 16:11:03 +0100 Subject: [PATCH 096/100] Fine tuning on the walking over steps --- .../main_walking_on_stairs.py | 50 ++++++++++--------- 1 file changed, 26 insertions(+), 24 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py index 038cf389..865b788a 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -89,9 +89,9 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S 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_gain = 150.0 settings.dcc_epsilon = 0.01 - settings.static_friction = 1.0 + 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 @@ -133,7 +133,6 @@ def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.S settings.casadi_solver_options = { "max_iter": 10000, "linear_solver": "mumps", - # "alpha_for_y": "dual-and-full", "fast_step_computation": "yes", "hessian_approximation": "limited-memory", "tol": 1e-3, @@ -403,7 +402,7 @@ def get_references( step_length = 0.9 step_height = 0.1 - swing_height = 1.0 + swing_height = 0.1 planner_settings = get_planner_settings( get_terrain(length=step_length / 2, width=0.8, height=step_height) @@ -488,11 +487,6 @@ def get_references( contact_guess=contact_phases_guess, ) - # print("Press [Enter] to visualize the initial state.") - # input() - # - # visualizer.visualize(initial_state) - final_state = compute_final_state( input_settings=planner_settings, pf_input=pf, @@ -500,33 +494,18 @@ def get_references( ) final_state.centroidal_momentum = np.zeros((6, 1)) - # print("Press [Enter] to visualize the final desired state.") - # input() - # - # visualizer.visualize(final_state) - second_state = compute_second_state( input_settings=planner_settings, pf_input=pf, contact_guess=contact_phases_guess, ) - # print("Press [Enter] to visualize the first intermediate state.") - # input() - # - # visualizer.visualize(second_state) - third_state = compute_third_state( input_settings=planner_settings, pf_input=pf, contact_guess=contact_phases_guess, ) - # print("Press [Enter] to visualize the second intermediate state.") - # input() - # - # visualizer.visualize(third_state) - # Until the middle of the second double support phase first_part_guess_length = (planner_settings.horizon_length * 5) // 14 first_part_guess = hp_rp.humanoid_state_interpolator( @@ -611,3 +590,26 @@ def get_references( save=True, file_name_stem="humanoid_walking_step", ) + + plotter_settings = hp_rp.FootContactStatePlotterSettings() + plotter_settings.terrain = planner_settings.terrain + left_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + right_foot_plotter = hp_rp.FootContactStatePlotter(plotter_settings) + + left_foot_plotter.plot_complementarity( + states=left_contact_points, + time_s=output.values.dt, + title="Left Foot Complementarity", + blocking=False, + ) + right_foot_plotter.plot_complementarity( + states=right_contact_points, + time_s=output.values.dt, + title="Right Foot Complementarity", + blocking=False, + ) + + print("Press [Enter] to exit.") + input() + left_foot_plotter.close() + right_foot_plotter.close() From fb574bcc336a52feca54616c26420e3967120f39 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 26 Jan 2024 18:13:11 +0100 Subject: [PATCH 097/100] Using resolve-robotics-uri-py instead of a parser --- .github/workflows/ci_cd.yml | 2 +- setup.cfg | 1 + .../humanoid_kinodynamic/main_periodic_step.py | 18 ++++++------------ .../main_single_step_flat_ground.py | 14 ++++---------- .../main_walking_on_stairs.py | 15 ++++----------- .../humanoid_pose_finder/main.py | 15 ++++----------- 6 files changed, 20 insertions(+), 45 deletions(-) diff --git a/.github/workflows/ci_cd.yml b/.github/workflows/ci_cd.yml index d00f5683..6d1db1f1 100644 --- a/.github/workflows/ci_cd.yml +++ b/.github/workflows/ci_cd.yml @@ -37,7 +37,7 @@ jobs: - name: Dependencies shell: bash -l {0} run: | - mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib + mamba install python=${{ matrix.python }} casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib resolve-robotics-uri-py mamba list - name: Install diff --git a/setup.cfg b/setup.cfg index 9f5c1adf..094d3d31 100644 --- a/setup.cfg +++ b/setup.cfg @@ -64,6 +64,7 @@ robot_planning= adam-robotics turnkey_planners= idyntree + resolve-robotics-uri-py visualization= ffmpeg-python idyntree diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 04563a8e..14e71664 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -1,10 +1,10 @@ -import argparse 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 @@ -15,17 +15,11 @@ def get_planner_settings() -> walking_settings.Settings: - parser = argparse.ArgumentParser( - description="Trajectory Optimization of periodic walking motion on ergoCub.", - ) - parser.add_argument( - "--urdf", - type=str, - required=True, - help="Path to the ergoCubGazeboV1_minContacts URDF file.", + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" ) settings = walking_settings.Settings() - settings.robot_urdf = parser.parse_args().urdf + settings.robot_urdf = str(urdf_path) settings.joints_name_list = [ "torso_pitch", "torso_roll", @@ -123,9 +117,9 @@ def get_planner_settings() -> walking_settings.Settings: "dual_inf_tol": 1000.0, "compl_inf_tol": 1e-2, "constr_viol_tol": 1e-4, - "acceptable_tol": 5e-1, + "acceptable_tol": 10, "acceptable_iter": 2, - "acceptable_compl_inf_tol": 1.0, + "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, diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py index 8dca28b1..31a47a83 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py @@ -1,10 +1,10 @@ -import argparse import logging import casadi as cs import idyntree.bindings as idyntree import liecasadi import numpy as np +import resolve_robotics_uri_py import hippopt.robot_planning as hp_rp import hippopt.turnkey_planners.humanoid_kinodynamic.planner as walking_planner @@ -14,17 +14,11 @@ def get_planner_settings() -> walking_settings.Settings: - parser = argparse.ArgumentParser( - description="Trajectory Optimization of a forward walking motion on ergoCub.", - ) - parser.add_argument( - "--urdf", - type=str, - required=True, - help="Path to the ergoCubGazeboV1_minContacts URDF file.", + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" ) settings = walking_settings.Settings() - settings.robot_urdf = parser.parse_args().urdf + settings.robot_urdf = str(urdf_path) settings.joints_name_list = [ "torso_pitch", "torso_roll", diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py index 865b788a..bfeb4762 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py @@ -1,10 +1,10 @@ -import argparse 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 @@ -28,19 +28,12 @@ def get_terrain(length: float, width: float, height: float) -> hp_rp.TerrainDesc def get_planner_settings(terrain: hp_rp.TerrainDescriptor) -> walking_settings.Settings: - parser = argparse.ArgumentParser( - description="Trajectory Optimization of a walking motion" - " on ergoCub over stairs.", - ) - parser.add_argument( - "--urdf", - type=str, - required=True, - help="Path to the ergoCubGazeboV1_minContacts URDF file.", + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" ) settings = walking_settings.Settings() settings.terrain = terrain - settings.robot_urdf = parser.parse_args().urdf + settings.robot_urdf = str(urdf_path) settings.joints_name_list = [ "torso_pitch", "torso_roll", diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py index 23ba4c95..b3548abd 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/main.py @@ -1,10 +1,10 @@ -import argparse import logging import casadi as cs import idyntree.bindings as idyntree import liecasadi import numpy as np +import resolve_robotics_uri_py import hippopt.robot_planning as hp_rp import hippopt.turnkey_planners.humanoid_pose_finder.planner as pose_finder @@ -12,19 +12,12 @@ if __name__ == "__main__": logging.basicConfig(level=logging.DEBUG) - parser = argparse.ArgumentParser( - description="Trajectory Optimization of a forward walking motion on ergoCub.", - ) - - parser.add_argument( - "--urdf", - type=str, - required=True, - help="Path to the ergoCubGazeboV1_minContacts URDF file.", + urdf_path = resolve_robotics_uri_py.resolve_robotics_uri( + "package://ergoCub/robots/ergoCubGazeboV1_minContacts/model.urdf" ) planner_settings = pose_finder.Settings() - planner_settings.robot_urdf = parser.parse_args().urdf + planner_settings.robot_urdf = str(urdf_path) planner_settings.joints_name_list = [ "torso_pitch", "torso_roll", From 1bc1256df83ff84087eb03e25e3b6a1ab90a88f5 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 26 Jan 2024 18:46:38 +0100 Subject: [PATCH 098/100] Updated README.md --- README.md | 31 ++++++++++++++++++++++++------- 1 file changed, 24 insertions(+), 7 deletions(-) diff --git a/README.md b/README.md index 3a917313..5cf54c9b 100644 --- a/README.md +++ b/README.md @@ -7,19 +7,36 @@ hippopt is an open-source framework for generating whole-body trajectories for l ## Features -- [ ] Direct transcription of optimal control problems with multiple-shooting methods -- [ ] Support for floating-base robots, including humanoids and quadrupeds -- [ ] Integration with CasADi library for efficient numerical optimization -- [ ] Generation of optimized trajectories that include both kinematic and dynamic quantities -- [ ] Extensive documentation and examples to help you get started +- [X] Direct transcription of optimal control problems with multiple-shooting methods +- [X] Support for floating-base robots, including humanoids ... + - [ ] ... and quadrupeds +- [X] Integration with CasADi library for efficient numerical optimization +- [X] Generation of optimized trajectories that include both kinematic and dynamic quantities +- [ ] Extensive documentation +- [X] examples to help you get started ## Installation -It is suggested to use [``conda``](https://docs.conda.io/en/latest/). +It is suggested to use [``mamba``](https://github.com/conda-forge/miniforge). ```bash -conda install -c conda-forge casadi pytest +conda install -c conda-forge -c robotology python=3.11 casadi pytest liecasadi adam-robotics idyntree meshcat-python ffmpeg-python matplotlib resolve-robotics-uri-py pip install --no-deps -e .[all] ``` +## Examples +### Turnkey planners +The folder [``turnkey_planners``](src/hippopt/turnkey_planners) contains examples of whole-body trajectory optimization for legged robots. +In this folder it is possible to find the following examples: +- [``humanoid_pose_finder/main.py``](src/hippopt/turnkey_planners/humanoid_pose_finder/main.py): generates a static pose for the humanoid robot ``ergoCub`` given desired foot and center of mass positions. +- [``humanoid_kinodynamic/main_single_step_flat_ground.py``](src/hippopt/turnkey_planners/humanoid_kinodynamic/main_single_step_flat_ground.py): generates a kinodynamic trajectory for the humanoid robot ``ergoCub`` to perform a single step motion with no a-priori guess or terminal constraint. +- [``humanoid_kinodynamic/main_periodic_step.py``](src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py): generates a kinodynamic trajectory for the humanoid robot ``ergoCub`` to perform a periodic walking motion. +- [``humanoid_kinodynamic/main_walking_on_stairs.py``](src/hippopt/turnkey_planners/humanoid_kinodynamic/main_walking_on_stairs.py): generates a kinodynamic trajectory for the humanoid robot ``ergoCub`` to perform a walking motion on stairs. + +> [!IMPORTANT] +> For the tests to run, it is necessary to clone [``ergocub-software``](https://github.com/icub-tech-iit/ergocub-software) and extend the ``GAZEBO_MODEL_PATH`` environment variable to include the ``ergocub-software/urdf/ergoCub/robots`` and ``ergocub-software/urdf`` folders. + +> [!NOTE] +> It is necessary to launch the examples from a folder with write permissions, as the examples will generate several files (ground meshes, output videos, ...). + ## Citing this work If you find the work useful, please consider citing: From 3e70dd0b73a9b9888fc44dc6d1cf1dccf801586f Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 26 Jan 2024 18:53:00 +0100 Subject: [PATCH 099/100] Updated black version --- .../humanoid_kinodynamic/planner.py | 20 ++++++++++--------- .../humanoid_kinodynamic/variables.py | 12 +++++------ .../humanoid_pose_finder/planner.py | 18 ++++++++--------- 3 files changed, 26 insertions(+), 24 deletions(-) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py index 43a36ba7..d72db04e 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/planner.py @@ -515,9 +515,11 @@ def _add_robot_dynamics(self, all_contact_points: list, function_inputs: dict): ) problem.add_dynamics( hp.dot(sym.system.centroidal_momentum) == centroidal_dynamics, # noqa - x0=problem.initial(sym.initial_state.centroidal_momentum) - if self.settings.periodicity_expression_type is hp.ExpressionType.skip - else None, # noqa + x0=( + problem.initial(sym.initial_state.centroidal_momentum) + if self.settings.periodicity_expression_type is hp.ExpressionType.skip + else None + ), # noqa dt=sym.dt, integrator=default_integrator, name="centroidal_momentum_dynamics", @@ -536,12 +538,12 @@ def _add_contact_kinematic_consistency( # Creation of contact kinematics consistency functions descriptor = point.descriptor if descriptor.foot_frame not in point_kinematics_functions: - point_kinematics_functions[ - descriptor.foot_frame - ] = hp_rp.point_position_from_kinematics( - kindyn_object=self.kin_dyn_object, - frame_name=descriptor.foot_frame, - **function_inputs, + point_kinematics_functions[descriptor.foot_frame] = ( + hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) ) # Consistency between the contact position and the kinematics diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py index 54ba3330..a0978f61 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/variables.py @@ -180,9 +180,9 @@ def from_feet_contact_points(self, input_points: hp_rp.FeetContactPoints) -> Non @dataclasses.dataclass class ExtendedHumanoid(hp.OptimizationObject): - contact_points: hp.CompositeType[ - FeetContactPointsExtended - ] = hp.default_composite_field(factory=FeetContactPointsExtended) + contact_points: hp.CompositeType[FeetContactPointsExtended] = ( + hp.default_composite_field(factory=FeetContactPointsExtended) + ) kinematics: hp.CompositeType[hp_rp.FloatingBaseSystem] = hp.default_composite_field( cls=hp.Variable, factory=hp_rp.FloatingBaseSystem @@ -295,9 +295,9 @@ 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] = ( + dataclasses.field(default=None) + ) def __post_init__( self, diff --git a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py index 3918d273..b6cdd972 100644 --- a/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py +++ b/src/hippopt/turnkey_planners/humanoid_pose_finder/planner.py @@ -164,9 +164,9 @@ class Variables(hp.OptimizationObject): minimum_joint_positions: hp.StorageType = hp.default_storage_field(hp.Parameter) settings: dataclasses.InitVar[Settings] = dataclasses.field(default=None) - kin_dyn_object: dataclasses.InitVar[ - adam.casadi.KinDynComputations - ] = dataclasses.field(default=None) + kin_dyn_object: dataclasses.InitVar[adam.casadi.KinDynComputations] = ( + dataclasses.field(default=None) + ) def __post_init__( self, @@ -455,12 +455,12 @@ def _add_contact_kinematic_consistency( # Creation of contact kinematics consistency functions descriptor = point.descriptor if descriptor.foot_frame not in point_kinematics_functions: - point_kinematics_functions[ - descriptor.foot_frame - ] = hp_rp.point_position_from_kinematics( - kindyn_object=self.kin_dyn_object, - frame_name=descriptor.foot_frame, - **function_inputs, + point_kinematics_functions[descriptor.foot_frame] = ( + hp_rp.point_position_from_kinematics( + kindyn_object=self.kin_dyn_object, + frame_name=descriptor.foot_frame, + **function_inputs, + ) ) # Consistency between the contact position and the kinematics From a37a6a2d5704d33818b2c9317342b989de1723c1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 23 Feb 2024 17:19:28 +0100 Subject: [PATCH 100/100] Fixed setting of initial desired CoM position in the periodic step --- .../turnkey_planners/humanoid_kinodynamic/main_periodic_step.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py index 14e71664..19f14c0b 100644 --- a/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py +++ b/src/hippopt/turnkey_planners/humanoid_kinodynamic/main_periodic_step.py @@ -267,6 +267,7 @@ def compute_initial_state( 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,