From b3a2ffe6a4ee106b0e583eb792f0d61e4da3ff30 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 19 Jul 2023 10:11:36 +0200 Subject: [PATCH 001/106] Added centroidal dynamics from point forces --- .../robot_planning/dynamics/__init__.py | 1 + .../robot_planning/dynamics/centroidal.py | 51 +++++++++++++++++++ 2 files changed, 52 insertions(+) create mode 100644 src/hippopt/robot_planning/dynamics/centroidal.py diff --git a/src/hippopt/robot_planning/dynamics/__init__.py b/src/hippopt/robot_planning/dynamics/__init__.py index e69de29b..be83d941 100644 --- a/src/hippopt/robot_planning/dynamics/__init__.py +++ b/src/hippopt/robot_planning/dynamics/__init__.py @@ -0,0 +1 @@ +from . import centroidal diff --git a/src/hippopt/robot_planning/dynamics/centroidal.py b/src/hippopt/robot_planning/dynamics/centroidal.py new file mode 100644 index 00000000..b8080951 --- /dev/null +++ b/src/hippopt/robot_planning/dynamics/centroidal.py @@ -0,0 +1,51 @@ +import casadi as cs + + +def centroidal_dynamics_with_point_forces( + number_of_points: int, + mass_name: str = "m", + gravity_name: str = "g", + com_name: str = "com", + point_position_name: str = "p_#", + point_force_name: str = "f_#", + options: dict = None, +) -> cs.Function: + if options is None: + options = dict() + + input_vars = [] + + m = cs.MX.sym(mass_name, 1) + input_vars.append(m) + + g = cs.MX.sym(gravity_name, 6) + input_vars.append(g) + + x = cs.MX.sym(com_name, 3) + input_vars.append(x) + + p = [] + f = [] + for i in range(number_of_points): + p.append(cs.MX.sym(point_position_name.replace("#", str(i)), 3)) + input_vars.append(p[i]) + f.append(cs.MX.sym(point_force_name.replace("#", str(i)), 3)) + input_vars.append(f[i]) + + input_names = [] + for var in input_vars: + input_names.append(var.name()) + + h_g = m @ g + + for i in range(number_of_points): + h_g = h_g + cs.vertcat(f[i], cs.cross(p[i] - x, f[i])) + + return cs.Function( + "centroidal_dynamics_with_point_forces", + input_vars, + [h_g], + input_names, + ["h_g"], + options, + ) From 06e5b426980814489414687fb079e758305736ec Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 19 Jul 2023 12:25:51 +0200 Subject: [PATCH 002/106] Added com dynamics --- .../robot_planning/dynamics/centroidal.py | 25 ++++++++++++++++--- 1 file changed, 22 insertions(+), 3 deletions(-) diff --git a/src/hippopt/robot_planning/dynamics/centroidal.py b/src/hippopt/robot_planning/dynamics/centroidal.py index b8080951..573e85dd 100644 --- a/src/hippopt/robot_planning/dynamics/centroidal.py +++ b/src/hippopt/robot_planning/dynamics/centroidal.py @@ -10,8 +10,7 @@ def centroidal_dynamics_with_point_forces( point_force_name: str = "f_#", options: dict = None, ) -> cs.Function: - if options is None: - options = dict() + options = {} if options is None else options input_vars = [] @@ -46,6 +45,26 @@ def centroidal_dynamics_with_point_forces( input_vars, [h_g], input_names, - ["h_g"], + ["h_g_dot"], + options, + ) + + +def com_dynamics_from_momentum( + mass_name: str = "m", momentum_name: str = "h_g", options: dict = None +) -> cs.Function: + options = {} if options is None else options + + m = cs.MX.sym(mass_name, 1) + h_g = cs.MX.sym(momentum_name, 6) + + x_dot = h_g[0:3] / m + + return cs.Function( + "com_dynamics_from_momentum", + [m, h_g], + [x_dot], + [mass_name, momentum_name], + ["x_dot"], options, ) From 6ce7964a689929e4377edcbaccb066e85bfffc6f Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 20 Jul 2023 20:22:20 +0200 Subject: [PATCH 003/106] Added function to compute the CMM from the robot state. --- .../robot_planning/dynamics/centroidal.py | 6 +- .../robot_planning/expressions/__init__.py | 1 + .../robot_planning/expressions/kinematics.py | 77 +++++++++++++++++++ .../robot_planning/utilities/__init__.py | 1 + .../robot_planning/utilities/quaternion.py | 30 ++++++++ 5 files changed, 114 insertions(+), 1 deletion(-) create mode 100644 src/hippopt/robot_planning/expressions/kinematics.py create mode 100644 src/hippopt/robot_planning/utilities/quaternion.py diff --git a/src/hippopt/robot_planning/dynamics/centroidal.py b/src/hippopt/robot_planning/dynamics/centroidal.py index 573e85dd..a61d8ab3 100644 --- a/src/hippopt/robot_planning/dynamics/centroidal.py +++ b/src/hippopt/robot_planning/dynamics/centroidal.py @@ -9,6 +9,7 @@ def centroidal_dynamics_with_point_forces( point_position_name: str = "p_#", point_force_name: str = "f_#", options: dict = None, + **_, ) -> cs.Function: options = {} if options is None else options @@ -51,7 +52,10 @@ def centroidal_dynamics_with_point_forces( def com_dynamics_from_momentum( - mass_name: str = "m", momentum_name: str = "h_g", options: dict = None + mass_name: str = "m", + momentum_name: str = "h_g", + options: dict = None, + **_, ) -> cs.Function: options = {} if options is None else options diff --git a/src/hippopt/robot_planning/expressions/__init__.py b/src/hippopt/robot_planning/expressions/__init__.py index e69de29b..43b0cdf1 100644 --- a/src/hippopt/robot_planning/expressions/__init__.py +++ b/src/hippopt/robot_planning/expressions/__init__.py @@ -0,0 +1 @@ +from . import kinematics diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py new file mode 100644 index 00000000..0c7d92d5 --- /dev/null +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -0,0 +1,77 @@ +import casadi as cs +import liecasadi +from adam.casadi import KinDynComputations + +from hippopt.robot_planning.utilities.quaternion import ( + quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, +) + + +def centroidal_momentum_from_kinematics( + kindyn_object: KinDynComputations, + base_position_name: str = "base_position", + base_quaternion_xyzw_name: str = "base_quaternion", + joint_positions_name: str = "joint_positions", + base_position_derivative_name: str = "base_position_derivative", + base_quaternion_xyzw_derivative_name: str = "base_quaternion_derivative", + joint_velocities_name: str = "joint_velocities", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + + base_position = cs.MX.sym(base_position_name, 3) + base_quaternion = cs.MX.sym(base_quaternion_xyzw_name, 4) + joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + base_position_derivative = cs.MX.sym(base_position_derivative_name, 3) + base_quaternion_derivative = cs.MX.sym(base_quaternion_xyzw_derivative_name, 4) + joint_velocities = cs.MX.sym(joint_velocities_name, kindyn_object.NDoF) + + cmm_function = kindyn_object.centroidal_momentum_matrix_fun() + + base_pose = liecasadi.SE3.from_position_quaternion( + base_position, base_quaternion + ).as_matrix() # The quaternion is supposed normalized + + base_angular_velocity_fun = ( + quaternion_xyzw_velocity_to_right_trivialized_angular_velocity( + quaternion_xyzw_name=base_quaternion_xyzw_name, + base_quaternion_xyzw_derivative_name=base_quaternion_xyzw_derivative_name, + options=options, + ) + ) + base_angular_velocity = base_angular_velocity_fun( + **{ + base_quaternion_xyzw_name: base_quaternion, + base_quaternion_xyzw_derivative_name: base_quaternion_derivative, + } + ) + + robot_velocity = cs.vertcat( + base_position_derivative, base_angular_velocity, joint_velocities + ) + + momentum = cmm_function(base_pose, joint_positions) @ robot_velocity + + return cs.Function( + "centroidal_momentum_from_kinematics", + [ + base_position, + base_quaternion, + joint_positions, + base_position_derivative, + base_quaternion_derivative, + joint_velocities, + ], + [momentum], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + base_position_derivative_name, + base_quaternion_xyzw_derivative_name, + joint_velocities_name, + ], + ["h_g"], + options, + ) diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index e69de29b..0e8b25d2 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -0,0 +1 @@ +from . import quaternion diff --git a/src/hippopt/robot_planning/utilities/quaternion.py b/src/hippopt/robot_planning/utilities/quaternion.py new file mode 100644 index 00000000..69109a52 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/quaternion.py @@ -0,0 +1,30 @@ +import casadi as cs + + +def quaternion_xyzw_velocity_to_right_trivialized_angular_velocity( + quaternion_xyzw_name: str = "quaternion", + quaternion_xyzw_velocity_name: str = "quaternion_velocity", + options: dict = None, + **_, +) -> cs.Function: + options = {} if options is None else options + quaternion = cs.MX.sym(quaternion_xyzw_name, 4) + quaternion_velocity = cs.MX.sym(quaternion_xyzw_velocity_name, 4) + + q_w = quaternion[3] + q_i = quaternion[1:3] + + q_dot_w = quaternion_velocity[3] + q_dot_i = quaternion_velocity[1:3] + + # See Sec. 1.5.3 of https://arxiv.org/pdf/0811.2889.pdf + angular_velocity = 2 * (-q_dot_w * q_i + q_w * q_dot_i - q_dot_i.cross(q_i)) + + return cs.Function( + "quaternion_xyzw_velocity_to_right_trivialized_angular_velocity", + [quaternion, quaternion_velocity], + [angular_velocity], + [quaternion_xyzw_name, quaternion_xyzw_velocity_name], + ["right_trivialized_angular_velocity"], + options, + ) From 4fc6505e39e53e1f9d11f4a0cedc20519c2a04d7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 24 Jul 2023 10:20:29 +0200 Subject: [PATCH 004/106] Added function to compute CoM position from kinematics --- .../robot_planning/expressions/kinematics.py | 40 +++++++++++++++++++ 1 file changed, 40 insertions(+) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index 0c7d92d5..5519ed03 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -75,3 +75,43 @@ def centroidal_momentum_from_kinematics( ["h_g"], options, ) + + +def center_of_mass_position_from_kinematics( + kindyn_object: KinDynComputations, + base_position_name: str = "base_position", + base_quaternion_xyzw_name: str = "base_quaternion", + joint_positions_name: str = "joint_positions", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + + base_position = cs.MX.sym(base_position_name, 3) + base_quaternion = cs.MX.sym(base_quaternion_xyzw_name, 4) + joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + + com_function = kindyn_object.CoM_position_fun() + + base_pose = liecasadi.SE3.from_position_quaternion( + base_position, base_quaternion + ).as_matrix() # The quaternion is supposed normalized + + com_position = com_function(base_pose, joint_positions) + + return cs.Function( + "center_of_mass_position_from_kinematics", + [ + base_position, + base_quaternion, + joint_positions, + ], + [com_position], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + ], + ["com_position"], + options, + ) From 4cc9b8e8a36962ea511cafead4786207a5f97bac Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 24 Jul 2023 12:33:21 +0200 Subject: [PATCH 005/106] Added function to compute ppoint position from kinematics --- .../robot_planning/expressions/kinematics.py | 47 +++++++++++++++++++ 1 file changed, 47 insertions(+) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index 5519ed03..17165553 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -115,3 +115,50 @@ def center_of_mass_position_from_kinematics( ["com_position"], options, ) + + +def point_position_from_kinematics( + kindyn_object: KinDynComputations, + frame_name: str, + point_position_in_frame_name: str = "point_position", + base_position_name: str = "base_position", + base_quaternion_xyzw_name: str = "base_quaternion", + joint_positions_name: str = "joint_positions", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + + base_position = cs.MX.sym(base_position_name, 3) + base_quaternion = cs.MX.sym(base_quaternion_xyzw_name, 4) + joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + point_position_in_frame = cs.MX.sym(point_position_in_frame_name, 3) + + fk_function = kindyn_object.forward_kinematics_fun(frame=frame_name) + + base_pose = liecasadi.SE3.from_position_quaternion( + base_position, base_quaternion + ).as_matrix() # The quaternion is supposed normalized + + frame_pose = fk_function(base_pose, joint_positions) + + point_position = frame_pose[:3, :3] @ point_position_in_frame + frame_pose[:3, 3] + + return cs.Function( + "point_position_from_kinematics", + [ + base_position, + base_quaternion, + joint_positions, + point_position_in_frame, + ], + [point_position], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + point_position_in_frame_name, + ], + ["point_position"], + options, + ) From 545885239d4a05a353011144433297c2418c41f1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 24 Jul 2023 12:40:55 +0200 Subject: [PATCH 006/106] Added quaternion normalization function --- .../robot_planning/utilities/quaternion.py | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/src/hippopt/robot_planning/utilities/quaternion.py b/src/hippopt/robot_planning/utilities/quaternion.py index 69109a52..e62db760 100644 --- a/src/hippopt/robot_planning/utilities/quaternion.py +++ b/src/hippopt/robot_planning/utilities/quaternion.py @@ -1,4 +1,25 @@ import casadi as cs +import liecasadi + + +def quaternion_xyzw_normalization( + quaternion_xyzw_name: str = "quaternion", + options: dict = None, + **_, +) -> cs.Function: + options = {} if options is None else options + quaternion = cs.MX.sym(quaternion_xyzw_name, 4) + + normalized_quaternion = liecasadi.Quaternion(xyzw=quaternion).normalize() + + return cs.Function( + "quaternion_xyzw_normalization", + [quaternion], + [normalized_quaternion.xyzw], + [quaternion_xyzw_name], + ["quaternion_normalized"], + options, + ) def quaternion_xyzw_velocity_to_right_trivialized_angular_velocity( From aa45a8cca3ede42b55192da10942d49b906bfd7d Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 24 Jul 2023 16:10:50 +0200 Subject: [PATCH 007/106] Added base class for a generic terrain description and a PlanarTerrain --- src/hippopt/robot_planning/__init__.py | 15 ++++ .../robot_planning/utilities/__init__.py | 2 +- .../utilities/terrain_descriptor.py | 70 +++++++++++++++++++ test/test_planar_terrain.py | 29 ++++++++ 4 files changed, 115 insertions(+), 1 deletion(-) create mode 100644 src/hippopt/robot_planning/__init__.py create mode 100644 src/hippopt/robot_planning/utilities/terrain_descriptor.py create mode 100644 test/test_planar_terrain.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py new file mode 100644 index 00000000..0cffb078 --- /dev/null +++ b/src/hippopt/robot_planning/__init__.py @@ -0,0 +1,15 @@ +from . import dynamics, expressions, utilities +from .dynamics.centroidal import ( + centroidal_dynamics_with_point_forces, + com_dynamics_from_momentum, +) +from .expressions.kinematics import ( + center_of_mass_position_from_kinematics, + centroidal_momentum_from_kinematics, + point_position_from_kinematics, +) +from .utilities.quaternion import ( + quaternion_xyzw_normalization, + quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, +) +from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index 0e8b25d2..f23da75e 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -1 +1 @@ -from . import quaternion +from . import quaternion, terrain_descriptor diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py new file mode 100644 index 00000000..9e8d9302 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -0,0 +1,70 @@ +import abc + +import casadi as cs + + +class TerrainDescriptor(abc.ABC): + @abc.abstractmethod + def height_function( + self, point_position_name: str = "point_position", options: dict = None + ) -> cs.Function: + pass + + @abc.abstractmethod + def normal_direction_function( + self, point_position_name: str = "point_position_name", options: dict = None + ) -> cs.Function: + pass + + @abc.abstractmethod + def orientation_function( + self, point_position_name: str = "point_position_name", options: dict = None + ) -> cs.Function: + pass + + +class PlanarTerrain(TerrainDescriptor): + def height_function( + self, point_position_name: str = "point_position", options: dict = None + ) -> cs.Function: + options = {} if options is None else options + point_position = cs.MX.sym(point_position_name, 3) + + return cs.Function( + "planar_terrain_height", + [point_position], + [point_position[2]], + [point_position_name], + ["point_height"], + options, + ) + + def normal_direction_function( + self, point_position_name: str = "point_position_name", options: dict = None + ) -> cs.Function: + options = {} if options is None else options + point_position = cs.MX.sym(point_position_name, 3) + + return cs.Function( + "planar_terrain_normal", + [point_position], + [cs.MX.eye(3)[:, 2]], + [point_position_name], + ["normal_direction"], + options, + ) + + def orientation_function( + self, point_position_name: str = "point_position_name", options: dict = None + ) -> cs.Function: + options = {} if options is None else options + point_position = cs.MX.sym(point_position_name, 3) + + return cs.Function( + "planar_terrain_orientation", + [point_position], + [cs.MX.eye(3)], + [point_position_name], + ["plane_rotation"], + options, + ) diff --git a/test/test_planar_terrain.py b/test/test_planar_terrain.py new file mode 100644 index 00000000..ea9d767e --- /dev/null +++ b/test/test_planar_terrain.py @@ -0,0 +1,29 @@ +import casadi as cs +import numpy as np + +import hippopt.robot_planning + + +def test_planar_terrain(): + planar_terrain = hippopt.robot_planning.PlanarTerrain() + + dummy_point = cs.DM.zeros(3) + dummy_point[2] = 0.5 + + height_function = planar_terrain.height_function(point_position_name="p") + + assert height_function(dummy_point) == 0.5 + + normal_function = planar_terrain.normal_direction_function() + normal_direction = np.zeros((3, 1)) + normal_direction[2] = 1.0 + output = normal_function(dummy_point).full() + + assert (normal_direction == output).all() + + orientation_fun = planar_terrain.orientation_function() + expected_orientation = np.eye(3) + + output = orientation_fun(dummy_point).full() + + assert (expected_orientation == output).all() # noqa From e0d5beee11885bf828039204b53a6a2b41cb0e44 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 24 Jul 2023 17:26:52 +0200 Subject: [PATCH 008/106] Small refactor of the terrain descriptor class to allow reusing the functions --- .../utilities/terrain_descriptor.py | 79 ++++++++++++------- test/test_planar_terrain.py | 10 ++- 2 files changed, 56 insertions(+), 33 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index 9e8d9302..ad4baa4e 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -1,70 +1,89 @@ import abc +import dataclasses import casadi as cs +@dataclasses.dataclass class TerrainDescriptor(abc.ABC): - @abc.abstractmethod - def height_function( + _height_function: cs.Function = dataclasses.field(default=None) + _normal_direction_function: cs.Function = dataclasses.field(default=None) + _orientation_function: cs.Function = dataclasses.field(default=None) + _point_position_name: str = dataclasses.field(default=None) + _options: dict = dataclasses.field(default=None) + point_position_name: dataclasses.InitVar[str] = dataclasses.field(default=None) + options: dataclasses.InitVar[dict] = dataclasses.field(default=None) + + def __post_init__( self, point_position_name: str = "point_position", options: dict = None - ) -> cs.Function: + ): + self._options = {} if options is None else options + self._point_position_name = point_position_name + + @abc.abstractmethod + def create_height_function(self) -> cs.Function: pass @abc.abstractmethod - def normal_direction_function( - self, point_position_name: str = "point_position_name", options: dict = None - ) -> cs.Function: + def create_normal_direction_function(self) -> cs.Function: pass @abc.abstractmethod - def orientation_function( - self, point_position_name: str = "point_position_name", options: dict = None - ) -> cs.Function: + def create_orientation_function(self) -> cs.Function: pass + def height_function(self) -> cs.Function: + if not isinstance(self._height_function, cs.Function): + self._height_function = self.create_height_function() + + return self._height_function + + def normal_direction_function(self) -> cs.Function: + if not isinstance(self._normal_direction_function, cs.Function): + self._normal_direction_function = self.create_normal_direction_function() + + return self._normal_direction_function + + def orientation_function(self) -> cs.Function: + if not isinstance(self._orientation_function, cs.Function): + self._orientation_function = self.create_orientation_function() + + return self._orientation_function + class PlanarTerrain(TerrainDescriptor): - def height_function( - self, point_position_name: str = "point_position", options: dict = None - ) -> cs.Function: - options = {} if options is None else options - point_position = cs.MX.sym(point_position_name, 3) + def create_height_function(self) -> cs.Function: + point_position = cs.MX.sym(self._point_position_name, 3) return cs.Function( "planar_terrain_height", [point_position], [point_position[2]], - [point_position_name], + [self._point_position_name], ["point_height"], - options, + self._options, ) - def normal_direction_function( - self, point_position_name: str = "point_position_name", options: dict = None - ) -> cs.Function: - options = {} if options is None else options - point_position = cs.MX.sym(point_position_name, 3) + def create_normal_direction_function(self) -> cs.Function: + point_position = cs.MX.sym(self._point_position_name, 3) return cs.Function( "planar_terrain_normal", [point_position], [cs.MX.eye(3)[:, 2]], - [point_position_name], + [self._point_position_name], ["normal_direction"], - options, + self._options, ) - def orientation_function( - self, point_position_name: str = "point_position_name", options: dict = None - ) -> cs.Function: - options = {} if options is None else options - point_position = cs.MX.sym(point_position_name, 3) + def create_orientation_function(self) -> cs.Function: + point_position = cs.MX.sym(self._point_position_name, 3) return cs.Function( "planar_terrain_orientation", [point_position], [cs.MX.eye(3)], - [point_position_name], + [self._point_position_name], ["plane_rotation"], - options, + self._options, ) diff --git a/test/test_planar_terrain.py b/test/test_planar_terrain.py index ea9d767e..9a8441f4 100644 --- a/test/test_planar_terrain.py +++ b/test/test_planar_terrain.py @@ -5,14 +5,18 @@ def test_planar_terrain(): - planar_terrain = hippopt.robot_planning.PlanarTerrain() + planar_terrain = hippopt.robot_planning.PlanarTerrain(point_position_name="p") dummy_point = cs.DM.zeros(3) dummy_point[2] = 0.5 - height_function = planar_terrain.height_function(point_position_name="p") + height_function = planar_terrain.height_function() - assert height_function(dummy_point) == 0.5 + assert next(iter(height_function(p=dummy_point).values())) == 0.5 + + assert ( + height_function is planar_terrain.height_function() + ) # Check that the function is created only once normal_function = planar_terrain.normal_direction_function() normal_direction = np.zeros((3, 1)) From ecef8ac63a0601ee995cee2c2c2e8c51da168c82 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 24 Jul 2023 18:18:05 +0200 Subject: [PATCH 009/106] Added expressions for friction and normal force positivity --- src/hippopt/robot_planning/__init__.py | 1 + .../robot_planning/expressions/contacts.py | 61 +++++++++++++++++++ .../utilities/terrain_descriptor.py | 3 + 3 files changed, 65 insertions(+) create mode 100644 src/hippopt/robot_planning/expressions/contacts.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 0cffb078..07bbaf37 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -3,6 +3,7 @@ centroidal_dynamics_with_point_forces, com_dynamics_from_momentum, ) +from .expressions.contacts import friction_cone_square_margin, normal_force_component from .expressions.kinematics import ( center_of_mass_position_from_kinematics, centroidal_momentum_from_kinematics, diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py new file mode 100644 index 00000000..1b0f407c --- /dev/null +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -0,0 +1,61 @@ +import casadi as cs + +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + + +def normal_force_component( + terrain: TerrainDescriptor, + point_force_name: str = "point_force", + options: dict = None, + **_ +): + options = {} if options is None else options + point_position = cs.MX.sym(terrain.get_point_position_name(), 3) + point_force = cs.MX.sym(point_force_name, 3) + + normal_direction_fun = terrain.normal_direction_function() + + normal_component = normal_direction_fun(point_position).T() @ point_force + + return cs.Function( + "normal_force_component", + [point_position, point_force], + [normal_component], + [terrain.get_point_position_name(), point_force_name], + ["normal_force"], + options, + ) + + +def friction_cone_square_margin( + terrain: TerrainDescriptor, + point_force_name: str = "point_force", + static_friction_name: str = "mu_s", + options: dict = None, + **_ +): + options = {} if options is None else options + point_position = cs.MX.sym(terrain.get_point_position_name(), 3) + point_force = cs.MX.sym(point_force_name, 3) + static_friction = cs.MX.sym(static_friction_name, 1) + + orientation_fun = terrain.orientation_function() + terrain_orientation = orientation_fun(point_position) + force_in_contact = terrain_orientation.T() @ point_force + + # In principle, it should be sqrt(fx^2 + fy^2) <= u * fz, + # but since both sides are positive, we square them both. + # Their difference needs to remain positive, i.e. + # (u * fz)^2 - (fx^2 + fy^2) >= 0 + margin = cs.sumsqr(static_friction * force_in_contact[2]) - cs.sumsqr( + force_in_contact[:2] + ) + + return cs.Function( + "friction_cone_square_margin", + [point_position, point_force, static_friction], + [margin], + [terrain.get_point_position_name(), point_force_name, static_friction_name], + ["margin"], + options, + ) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index ad4baa4e..45992866 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -50,6 +50,9 @@ def orientation_function(self) -> cs.Function: return self._orientation_function + def get_point_position_name(self) -> str: + return self._point_position_name + class PlanarTerrain(TerrainDescriptor): def create_height_function(self) -> cs.Function: From 783604a0aba855d8b2e4bbfb388a16ff98600da0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 25 Jul 2023 17:52:51 +0200 Subject: [PATCH 010/106] Changed expression for the static friction margin --- .../robot_planning/expressions/contacts.py | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index 1b0f407c..b3608a5d 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -29,13 +29,19 @@ def normal_force_component( def friction_cone_square_margin( terrain: TerrainDescriptor, + point_position_name: str = None, point_force_name: str = "point_force", static_friction_name: str = "mu_s", options: dict = None, **_ ): options = {} if options is None else options - point_position = cs.MX.sym(terrain.get_point_position_name(), 3) + point_position_name = ( + terrain.get_point_position_name() + if point_position_name is None + else point_position_name + ) + point_position = cs.MX.sym(point_position_name, 3) point_force = cs.MX.sym(point_force_name, 3) static_friction = cs.MX.sym(static_friction_name, 1) @@ -47,15 +53,17 @@ def friction_cone_square_margin( # but since both sides are positive, we square them both. # Their difference needs to remain positive, i.e. # (u * fz)^2 - (fx^2 + fy^2) >= 0 - margin = cs.sumsqr(static_friction * force_in_contact[2]) - cs.sumsqr( - force_in_contact[:2] + # that is equal to + # [-1, -1, u^2] * f.^2 + margin = cs.horzcat([-1, -1, cs.constpow(static_friction, 2)]) * cs.constpow( + force_in_contact, 2 ) return cs.Function( "friction_cone_square_margin", [point_position, point_force, static_friction], [margin], - [terrain.get_point_position_name(), point_force_name, static_friction_name], + [point_position_name, point_force_name, static_friction_name], ["margin"], options, ) From 79641aa3a36417403fc090d6e31eb8d2e495b748 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 26 Jul 2023 14:19:59 +0200 Subject: [PATCH 011/106] Added first version of complementarity functions --- .../expressions/complementarity.py | 109 ++++++++++++++++++ .../robot_planning/expressions/contacts.py | 14 ++- 2 files changed, 119 insertions(+), 4 deletions(-) create mode 100644 src/hippopt/robot_planning/expressions/complementarity.py diff --git a/src/hippopt/robot_planning/expressions/complementarity.py b/src/hippopt/robot_planning/expressions/complementarity.py new file mode 100644 index 00000000..99ce886c --- /dev/null +++ b/src/hippopt/robot_planning/expressions/complementarity.py @@ -0,0 +1,109 @@ +import casadi as cs + +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + + +def dcc_planar_complementarity( + terrain: TerrainDescriptor, + point_position_name: str = None, + height_multiplier_name: str = "kt", + point_position_control_name: str = "u_p", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + point_position_name = ( + terrain.get_point_position_name() + if point_position_name is None + else point_position_name + ) + point_position = cs.MX.sym(point_position_name, 3) + height_multiplier = cs.MX.sym(height_multiplier_name, 1) + point_control = cs.MX.sym(point_position_control_name, 3) + + terrain_orientation_fun = terrain.orientation_function() + height_function = terrain.height_function() + + height = height_function(point_position) + terrain_orientation = terrain_orientation_fun(point_position) + + planar_multiplier = cs.tanh(height_multiplier * height) + multipliers = cs.diag(cs.horzcat([planar_multiplier, planar_multiplier, 1])) + planar_complementarity = terrain_orientation @ multipliers @ point_control + + return cs.Function( + "planar_complementarity_dcc", + [point_position, height_multiplier], + [planar_complementarity], + [point_position_name, height_multiplier_name, point_position_control_name], + ["planar_complementarity"], + options, + ) + + +def dcc_complementarity_margin( + terrain: TerrainDescriptor, + point_position_name: str = None, + point_force_name: str = "point_force", + point_velocity_name: str = "point_velocity", + point_force_derivative_name: str = "point_force", + dcc_gain_name: str = "k_bs", + epsilon_name: str = "eps", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + point_position_name = ( + terrain.get_point_position_name() + if point_position_name is None + else point_position_name + ) + point_position = cs.MX.sym(point_position_name, 3) + point_force = cs.MX.sym(point_force_name, 3) + point_velocity = cs.MX.sym(point_velocity_name, 3) + point_force_derivative = cs.MX.sym(point_force_derivative_name, 3) + dcc_gain = cs.MX.sym(dcc_gain_name, 1) + eps = cs.MX.sym(epsilon_name, 1) + + normal_direction_fun = terrain.normal_direction_function() + height_function = terrain.height_function() + + height = height_function(point_position) + normal_direction = normal_direction_fun(point_position) + + # See Sec III.A of https://ieeexplore.ieee.org/abstract/document/9847574 + height_derivative = cs.jtimes(height, point_position, point_velocity) + normal_derivative = cs.jtimes(normal_direction, point_position, point_velocity) + normal_force = normal_direction.T() @ point_force + normal_force_derivative = normal_direction.T() @ point_force_derivative + complementarity = height * normal_force + + csi = ( + height_derivative * normal_force + + height * point_force.T() @ normal_derivative + + height * normal_force_derivative + ) + + margin = eps - dcc_gain * complementarity - csi + + return cs.Function( + "dcc_complementarity_margin", + [ + point_position, + point_force, + point_velocity, + point_force_derivative, + dcc_gain, + eps, + ], + [margin], + [ + point_position_name, + point_force_name, + point_velocity_name, + point_force_derivative_name, + dcc_gain_name, + epsilon_name, + ], + ["dcc_complementarity_margin"], + ) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index b3608a5d..db2db576 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -5,12 +5,18 @@ def normal_force_component( terrain: TerrainDescriptor, + point_position_name: str = None, point_force_name: str = "point_force", options: dict = None, **_ -): +) -> cs.Function: options = {} if options is None else options - point_position = cs.MX.sym(terrain.get_point_position_name(), 3) + point_position_name = ( + terrain.get_point_position_name() + if point_position_name is None + else point_position_name + ) + point_position = cs.MX.sym(point_position_name, 3) point_force = cs.MX.sym(point_force_name, 3) normal_direction_fun = terrain.normal_direction_function() @@ -21,7 +27,7 @@ def normal_force_component( "normal_force_component", [point_position, point_force], [normal_component], - [terrain.get_point_position_name(), point_force_name], + [point_position_name, point_force_name], ["normal_force"], options, ) @@ -34,7 +40,7 @@ def friction_cone_square_margin( static_friction_name: str = "mu_s", options: dict = None, **_ -): +) -> cs.Function: options = {} if options is None else options point_position_name = ( terrain.get_point_position_name() From 00619b1f59463911cb6ef1f7415f4422b74fa343 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 27 Jul 2023 12:14:54 +0200 Subject: [PATCH 012/106] Added function to compute two frame relative position --- .../robot_planning/expressions/kinematics.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index 17165553..7e567d52 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -162,3 +162,38 @@ def point_position_from_kinematics( ["point_position"], options, ) + + +def frames_relative_position( + kindyn_object: KinDynComputations, + reference_frame: str, + target_frame: str, + joint_positions_name: str = "joint_positions", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + + base_pose = cs.DM_eye(4) + + reference_fk_function = kindyn_object.forward_kinematics_fun(frame=reference_frame) + target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) + + reference_pose = liecasadi.SE3.from_matrix( + reference_fk_function(base_pose, joint_positions) + ) + reference_pose_inverse = reference_pose.inverse() + target_pose = target_fk_function(base_pose, joint_positions) + + relative_position = ( + reference_pose_inverse.rotation().act(target_pose[:3, 3]) + + reference_pose_inverse.translation() + ) + + return cs.Function( + "frames_relative_position", + [joint_positions], + [relative_position], + "relative_position", + ) From 3910ebf6fe6f394f5947d421787d9bbe985326fc Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 27 Jul 2023 18:25:53 +0200 Subject: [PATCH 013/106] Added missing options in kinematics and complementarity --- src/hippopt/robot_planning/expressions/complementarity.py | 1 + src/hippopt/robot_planning/expressions/kinematics.py | 1 + 2 files changed, 2 insertions(+) diff --git a/src/hippopt/robot_planning/expressions/complementarity.py b/src/hippopt/robot_planning/expressions/complementarity.py index 99ce886c..d559db46 100644 --- a/src/hippopt/robot_planning/expressions/complementarity.py +++ b/src/hippopt/robot_planning/expressions/complementarity.py @@ -106,4 +106,5 @@ def dcc_complementarity_margin( epsilon_name, ], ["dcc_complementarity_margin"], + options, ) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index 7e567d52..cccd5ce4 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -196,4 +196,5 @@ def frames_relative_position( [joint_positions], [relative_position], "relative_position", + options, ) From 05de558f67a75050f2f1117e5f95f9a88b90f6eb Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 1 Aug 2023 18:03:19 +0200 Subject: [PATCH 014/106] Added contact points centroid function --- .../robot_planning/expressions/contacts.py | 33 +++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index db2db576..160470d9 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -73,3 +73,36 @@ def friction_cone_square_margin( ["margin"], options, ) + + +def contact_points_centroid( + number_of_points: int, point_position_name: str = "p_#", options: dict = None, **_ +) -> cs.Function: + options = {} if options is None else options + + input_vars = [] + p = [] + for i in range(number_of_points): + p.append(cs.MX.sym(point_position_name.replace("#", str(i)), 3)) + input_vars.append(p[i]) + + input_names = [] + for var in input_vars: + input_names.append(var.name()) + + centroid = cs.DM.zeros(3, 1) + + for point in p: + centroid = centroid + point + + if number_of_points > 0: + centroid = centroid / number_of_points + + return cs.Function( + "contact_points_centroid", + input_vars, + [centroid], + input_names, + ["centroid"], + options, + ) From ff172a513ba2a1840cf34234fbc8bcbd12640fcf Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 2 Aug 2023 11:56:13 +0200 Subject: [PATCH 015/106] Added contact_points_yaw_alignment and swing_height_heuristic --- .../robot_planning/expressions/contacts.py | 59 +++++++++++++++++++ 1 file changed, 59 insertions(+) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index 160470d9..b57041b8 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -106,3 +106,62 @@ def contact_points_centroid( ["centroid"], options, ) + + +def contact_points_yaw_alignment( + first_point_name: str = "p_0", + second_point_name: str = "p_1", + desired_yaw_name: str = "desired_yaw", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + + p0 = cs.MX.sym(first_point_name, 3) + p1 = cs.MX.sym(second_point_name, 3) + yaw = cs.MX.sym(desired_yaw_name, 1) + + yaw_alignment = cs.horzcat([-cs.sin(yaw), cs.cos(yaw)]) @ (p1 - p0)[:2] + + return cs.Function( + "contact_points_yaw_alignment", + [p0, p1, yaw], + [yaw_alignment], + [first_point_name, second_point_name, desired_yaw_name], + ["yaw_alignment"], + options, + ) + + +def swing_height_heuristic( + terrain: TerrainDescriptor, + point_name: str = "p", + point_velocity_name: str = "p_dot", + desired_height_name: str = "h_desired", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + + point = cs.MX.sym(point_name, 3) + point_velocity = cs.MX.sym(point_velocity_name, 3) + desired_height = cs.MX.sym(desired_height_name, 1) + + height_fun = terrain.height_function() + terrain_height = height_fun(point) + terrain_orientation_fun = terrain.orientation_function() + terrain_orientation = terrain_orientation_fun(point) + + height_difference = terrain_height - desired_height + planar_velocity = (terrain_orientation.T() @ point_velocity)[:2] + + heuristic = 0.5 * (cs.constpow(height_difference, 2) + cs.sumsqr(planar_velocity)) + + return cs.Function( + "swing_height_heuristic", + [point, point_velocity, desired_height], + [heuristic], + [point_name, point_velocity_name, desired_height_name], + ["heuristic"], + options, + ) From 124908f526eac1cd6e3b0408503aa3506b4361de Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 2 Aug 2023 11:56:35 +0200 Subject: [PATCH 016/106] Added quaternion_error function --- .../robot_planning/expressions/kinematics.py | 51 +++++++++++++++++++ test/test_planar_terrain.py | 2 +- 2 files changed, 52 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index cccd5ce4..ad648663 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -198,3 +198,54 @@ def frames_relative_position( "relative_position", options, ) + + +def quaternion_error( + kindyn_object: KinDynComputations, + target_frame: str, + base_position_name: str = "base_position", + base_quaternion_xyzw_name: str = "base_quaternion", + joint_positions_name: str = "joint_positions", + desired_quaternion_xyzw_name: str = "desired_quaternion", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + base_position = cs.MX.sym(base_position_name, 3) + base_quaternion = cs.MX.sym(base_quaternion_xyzw_name, 4) + joint_positions = cs.MX.sym(joint_positions_name, kindyn_object.NDoF) + desired_quaternion = cs.MX.sym(desired_quaternion_xyzw_name, 4) + + base_pose = liecasadi.SE3.from_position_quaternion( + base_position, base_quaternion + ).as_matrix() # The quaternion is supposed normalized + + target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) + target_pose = target_fk_function(base_pose, joint_positions) + + target_rotation = liecasadi.SO3.from_matrix(target_pose[:3, :3]) + desired_rotation = liecasadi.SO3.from_quat(desired_quaternion) + + rotation_error = desired_rotation.inverse() * target_rotation + identity = liecasadi.SO3.Identity() + + error = rotation_error.as_quat() - identity.as_quat() + + return cs.Function( + "quaternion_error", + [ + base_position, + base_quaternion, + joint_positions, + desired_quaternion, + ], + [error], + [ + base_position_name, + base_quaternion_xyzw_name, + joint_positions_name, + desired_quaternion_xyzw_name, + ], + ["quaternion_error"], + options, + ) diff --git a/test/test_planar_terrain.py b/test/test_planar_terrain.py index 9a8441f4..87d10449 100644 --- a/test/test_planar_terrain.py +++ b/test/test_planar_terrain.py @@ -23,7 +23,7 @@ def test_planar_terrain(): normal_direction[2] = 1.0 output = normal_function(dummy_point).full() - assert (normal_direction == output).all() + assert (normal_direction == output).all() # noqa orientation_fun = planar_terrain.orientation_function() expected_orientation = np.eye(3) From 799b89874e95202c08bdb999549c0e6eda7f8ed8 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 2 Aug 2023 15:55:30 +0200 Subject: [PATCH 017/106] Added ContactPoint variable definition --- src/hippopt/robot_planning/__init__.py | 15 +++++- .../robot_planning/expressions/__init__.py | 2 +- .../robot_planning/variables/__init__.py | 1 + .../robot_planning/variables/contacts.py | 52 +++++++++++++++++++ 4 files changed, 68 insertions(+), 2 deletions(-) create mode 100644 src/hippopt/robot_planning/variables/contacts.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 07bbaf37..13bfbe9b 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -3,14 +3,27 @@ centroidal_dynamics_with_point_forces, com_dynamics_from_momentum, ) -from .expressions.contacts import friction_cone_square_margin, normal_force_component +from .expressions.complementarity import ( + dcc_complementarity_margin, + dcc_planar_complementarity, +) +from .expressions.contacts import ( + contact_points_centroid, + contact_points_yaw_alignment, + friction_cone_square_margin, + normal_force_component, + swing_height_heuristic, +) from .expressions.kinematics import ( center_of_mass_position_from_kinematics, centroidal_momentum_from_kinematics, + frames_relative_position, point_position_from_kinematics, + quaternion_error, ) from .utilities.quaternion import ( quaternion_xyzw_normalization, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor +from .variables.contacts import ContactPoint, ContactPointDescriptor diff --git a/src/hippopt/robot_planning/expressions/__init__.py b/src/hippopt/robot_planning/expressions/__init__.py index 43b0cdf1..9593c935 100644 --- a/src/hippopt/robot_planning/expressions/__init__.py +++ b/src/hippopt/robot_planning/expressions/__init__.py @@ -1 +1 @@ -from . import kinematics +from . import complementarity, contacts, kinematics diff --git a/src/hippopt/robot_planning/variables/__init__.py b/src/hippopt/robot_planning/variables/__init__.py index e69de29b..1bfdb36c 100644 --- a/src/hippopt/robot_planning/variables/__init__.py +++ b/src/hippopt/robot_planning/variables/__init__.py @@ -0,0 +1 @@ +from . import contacts diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py new file mode 100644 index 00000000..f1c2e1c1 --- /dev/null +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -0,0 +1,52 @@ +import dataclasses + +import numpy as np + +from hippopt import ( + OptimizationObject, + Parameter, + StorageType, + Variable, + default_storage_field, +) + + +@dataclasses.dataclass +class ContactPointDescriptor(OptimizationObject): + position_in_foot_frame: StorageType = default_storage_field(Parameter) + foot_frame: str = dataclasses.field(default=None) + + input_foot_frame: dataclasses.InitVar[str] = dataclasses.field(default=None) + input_position_in_foot_frame: dataclasses.InitVar[np.ndarray] = dataclasses.field( + default=None + ) + + def __post_init__( + self, input_foot_frame: str, input_position_in_foot_frame: np.ndarray + ): + self.foot_frame = input_foot_frame + self.position_in_foot_frame = input_position_in_foot_frame + + +@dataclasses.dataclass +class ContactPoint(OptimizationObject): + p: StorageType = default_storage_field(Variable) + v: StorageType = default_storage_field(Variable) + u_v: StorageType = default_storage_field(Variable) + f: StorageType = default_storage_field(Variable) + f_dot: StorageType = default_storage_field(Variable) + + descriptor: ContactPointDescriptor = dataclasses.field(default=None) + + input_descriptor: dataclasses.InitVar[ContactPointDescriptor] = dataclasses.field( + default=None + ) + + def __post_init__(self, input_descriptor: ContactPointDescriptor): + self.p = np.zeros(3) + self.v = np.zeros(3) + self.u_v = np.zeros(3) + self.f = np.zeros(3) + self.f_dot = np.zeros(3) + + self.descriptor = input_descriptor From ebc9785767b91bdf4f05e95c335caf008189ff79 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 12:36:39 +0200 Subject: [PATCH 018/106] Added initial conditions in contact variables. Removed u_v --- src/hippopt/robot_planning/variables/contacts.py | 16 +++++++++++----- 1 file changed, 11 insertions(+), 5 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index f1c2e1c1..fd2193c9 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -7,6 +7,7 @@ Parameter, StorageType, Variable, + default_composite_field, default_storage_field, ) @@ -23,7 +24,7 @@ class ContactPointDescriptor(OptimizationObject): def __post_init__( self, input_foot_frame: str, input_position_in_foot_frame: np.ndarray - ): + ) -> None: self.foot_frame = input_foot_frame self.position_in_foot_frame = input_position_in_foot_frame @@ -32,21 +33,26 @@ def __post_init__( class ContactPoint(OptimizationObject): p: StorageType = default_storage_field(Variable) v: StorageType = default_storage_field(Variable) - u_v: StorageType = default_storage_field(Variable) f: StorageType = default_storage_field(Variable) f_dot: StorageType = default_storage_field(Variable) - descriptor: ContactPointDescriptor = dataclasses.field(default=None) + # Initial conditions + p0: StorageType = default_storage_field(Parameter) + v0: StorageType = default_storage_field(Parameter) + + descriptor: ContactPointDescriptor = default_composite_field(time_varying=False) input_descriptor: dataclasses.InitVar[ContactPointDescriptor] = dataclasses.field( default=None ) - def __post_init__(self, input_descriptor: ContactPointDescriptor): + def __post_init__(self, input_descriptor: ContactPointDescriptor) -> None: self.p = np.zeros(3) self.v = np.zeros(3) - self.u_v = np.zeros(3) self.f = np.zeros(3) self.f_dot = np.zeros(3) + self.p0 = np.zeros(3) + self.v0 = np.zeros(3) + self.descriptor = input_descriptor From 853e4761d1b9fbafef15cacc6f101a549743e0b4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 13:42:58 +0200 Subject: [PATCH 019/106] Added variables describing a floating base system --- src/hippopt/robot_planning/__init__.py | 5 ++ .../robot_planning/variables/__init__.py | 2 +- .../robot_planning/variables/floating_base.py | 63 +++++++++++++++++++ 3 files changed, 69 insertions(+), 1 deletion(-) create mode 100644 src/hippopt/robot_planning/variables/floating_base.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 13bfbe9b..28a5d941 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -27,3 +27,8 @@ ) from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor from .variables.contacts import ContactPoint, ContactPointDescriptor +from .variables.floating_base import ( + FloatingBaseSystem, + FreeFloatingObject, + KinematicTree, +) diff --git a/src/hippopt/robot_planning/variables/__init__.py b/src/hippopt/robot_planning/variables/__init__.py index 1bfdb36c..30640706 100644 --- a/src/hippopt/robot_planning/variables/__init__.py +++ b/src/hippopt/robot_planning/variables/__init__.py @@ -1 +1 @@ -from . import contacts +from . import contacts, floating_base diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py new file mode 100644 index 00000000..f99b9c34 --- /dev/null +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -0,0 +1,63 @@ +import dataclasses + +import numpy as np + +from hippopt import ( + OptimizationObject, + Parameter, + StorageType, + Variable, + default_composite_field, + default_storage_field, +) + + +@dataclasses.dataclass +class FreeFloatingObject(OptimizationObject): + position: StorageType = default_storage_field(Variable) + linear_velocity: StorageType = default_storage_field(Variable) + quaternion_xyzw: StorageType = default_storage_field(Variable) + quaternion_velocity_xyzw: StorageType = default_storage_field(Variable) + + # Initial conditions + initial_position: StorageType = default_storage_field(Parameter) + initial_quaternion_xyzw: StorageType = default_storage_field(Parameter) + + def __post_init__(self): + self.position = np.zeros(3) + self.linear_velocity = np.zeros(3) + self.quaternion_xyzw = np.zeros(4) + self.quaternion_xyzw[3] = 1.0 + self.quaternion_velocity_xyzw = np.zeros(4) + + self.initial_position = np.zeros(3) + self.initial_quaternion_xyzw = np.zeros(4) + self.initial_quaternion_xyzw[3] = 1.0 + + +@dataclasses.dataclass +class KinematicTree(OptimizationObject): + positions: StorageType = default_storage_field(Variable) + velocities: StorageType = default_storage_field(Variable) + + # Initial conditions + initial_positions: StorageType = default_storage_field(Parameter) + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_joints: int): + self.positions = np.zeros(number_of_joints) + self.velocities = np.zeros(number_of_joints) + + self.initial_positions = np.zeros(number_of_joints) + + +@dataclasses.dataclass +class FloatingBaseSystem(OptimizationObject): + base: FreeFloatingObject = default_composite_field(factory=FreeFloatingObject) + joints: KinematicTree = default_composite_field() + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_joints: int): + self.joints = KinematicTree(number_of_joints=number_of_joints) From 6cb1bc2379924839b5b20875154bfc09b06dce05 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 16:17:34 +0200 Subject: [PATCH 020/106] Added f0 initial condition in ContactPoint variable --- src/hippopt/robot_planning/variables/contacts.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index fd2193c9..50abd513 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -39,6 +39,7 @@ class ContactPoint(OptimizationObject): # Initial conditions p0: StorageType = default_storage_field(Parameter) v0: StorageType = default_storage_field(Parameter) + f0: StorageType = default_storage_field(Parameter) descriptor: ContactPointDescriptor = default_composite_field(time_varying=False) @@ -54,5 +55,6 @@ def __post_init__(self, input_descriptor: ContactPointDescriptor) -> None: self.p0 = np.zeros(3) self.v0 = np.zeros(3) + self.f0 = np.zeros(3) self.descriptor = input_descriptor From 741da06e113eb88bac58abda697bb3ed9b939223 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 17:46:25 +0200 Subject: [PATCH 021/106] Allowing to set the individual name of the variables in the centroidal dynamics function --- .../robot_planning/dynamics/centroidal.py | 22 +++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/hippopt/robot_planning/dynamics/centroidal.py b/src/hippopt/robot_planning/dynamics/centroidal.py index a61d8ab3..f9000d6d 100644 --- a/src/hippopt/robot_planning/dynamics/centroidal.py +++ b/src/hippopt/robot_planning/dynamics/centroidal.py @@ -6,13 +6,27 @@ def centroidal_dynamics_with_point_forces( mass_name: str = "m", gravity_name: str = "g", com_name: str = "com", - point_position_name: str = "p_#", - point_force_name: str = "f_#", + point_position_names: list[str] = None, + point_force_names: list[str] = None, options: dict = None, **_, ) -> cs.Function: options = {} if options is None else options + if point_position_names is None: + point_position_names = [] + for i in range(number_of_points): + point_position_names.append(f"p{i}") + + assert len(point_position_names) == number_of_points + + if point_force_names is None: + point_force_names = [] + for i in range(number_of_points): + point_force_names.append(f"f{i}") + + assert len(point_force_names) == number_of_points + input_vars = [] m = cs.MX.sym(mass_name, 1) @@ -27,9 +41,9 @@ def centroidal_dynamics_with_point_forces( p = [] f = [] for i in range(number_of_points): - p.append(cs.MX.sym(point_position_name.replace("#", str(i)), 3)) + p.append(cs.MX.sym(point_position_names[i], 3)) input_vars.append(p[i]) - f.append(cs.MX.sym(point_force_name.replace("#", str(i)), 3)) + f.append(cs.MX.sym(point_force_names[i], 3)) input_vars.append(f[i]) input_names = [] From 919c724b41b63748621d485fd2ddbc3e0b4b965c Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 7 Aug 2023 18:24:26 +0200 Subject: [PATCH 022/106] Fixed missing input in planar complementarity function --- src/hippopt/robot_planning/expressions/complementarity.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/expressions/complementarity.py b/src/hippopt/robot_planning/expressions/complementarity.py index d559db46..eb90eb3f 100644 --- a/src/hippopt/robot_planning/expressions/complementarity.py +++ b/src/hippopt/robot_planning/expressions/complementarity.py @@ -33,7 +33,7 @@ def dcc_planar_complementarity( return cs.Function( "planar_complementarity_dcc", - [point_position, height_multiplier], + [point_position, height_multiplier, point_control], [planar_complementarity], [point_position_name, height_multiplier_name, point_position_control_name], ["planar_complementarity"], From d41f784bdf5840b4c21013d554bea871047bd9ae Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 11:39:44 +0200 Subject: [PATCH 023/106] Added dcc complementarity to flat ground planner Renamed a parameter in dcc_complementarity_margin for better clarity --- src/hippopt/robot_planning/expressions/complementarity.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/complementarity.py b/src/hippopt/robot_planning/expressions/complementarity.py index eb90eb3f..3946df49 100644 --- a/src/hippopt/robot_planning/expressions/complementarity.py +++ b/src/hippopt/robot_planning/expressions/complementarity.py @@ -46,9 +46,9 @@ def dcc_complementarity_margin( point_position_name: str = None, point_force_name: str = "point_force", point_velocity_name: str = "point_velocity", - point_force_derivative_name: str = "point_force", + point_force_derivative_name: str = "point_force_derivative", dcc_gain_name: str = "k_bs", - epsilon_name: str = "eps", + dcc_epsilon_name: str = "eps", options: dict = None, **_ ) -> cs.Function: @@ -63,7 +63,7 @@ def dcc_complementarity_margin( point_velocity = cs.MX.sym(point_velocity_name, 3) point_force_derivative = cs.MX.sym(point_force_derivative_name, 3) dcc_gain = cs.MX.sym(dcc_gain_name, 1) - eps = cs.MX.sym(epsilon_name, 1) + eps = cs.MX.sym(dcc_epsilon_name, 1) normal_direction_fun = terrain.normal_direction_function() height_function = terrain.height_function() @@ -103,7 +103,7 @@ def dcc_complementarity_margin( point_velocity_name, point_force_derivative_name, dcc_gain_name, - epsilon_name, + dcc_epsilon_name, ], ["dcc_complementarity_margin"], options, From c1154a42cba7bfd8ddb24488de84b55e1db33499 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 11:48:47 +0200 Subject: [PATCH 024/106] Added possibility to change the options of a terrain descriptor after its creation --- .../utilities/terrain_descriptor.py | 17 +++++++++++------ 1 file changed, 11 insertions(+), 6 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index 45992866..92594d07 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -16,6 +16,11 @@ class TerrainDescriptor(abc.ABC): def __post_init__( self, point_position_name: str = "point_position", options: dict = None + ): + self.change_options(point_position_name, options) + + def change_options( + self, point_position_name: str = "point_position", options: dict = None, **_ ): self._options = {} if options is None else options self._point_position_name = point_position_name @@ -56,37 +61,37 @@ def get_point_position_name(self) -> str: class PlanarTerrain(TerrainDescriptor): def create_height_function(self) -> cs.Function: - point_position = cs.MX.sym(self._point_position_name, 3) + point_position = cs.MX.sym(self.get_point_position_name(), 3) return cs.Function( "planar_terrain_height", [point_position], [point_position[2]], - [self._point_position_name], + [self.get_point_position_name()], ["point_height"], self._options, ) def create_normal_direction_function(self) -> cs.Function: - point_position = cs.MX.sym(self._point_position_name, 3) + point_position = cs.MX.sym(self.get_point_position_name(), 3) return cs.Function( "planar_terrain_normal", [point_position], [cs.MX.eye(3)[:, 2]], - [self._point_position_name], + [self.get_point_position_name()], ["normal_direction"], self._options, ) def create_orientation_function(self) -> cs.Function: - point_position = cs.MX.sym(self._point_position_name, 3) + point_position = cs.MX.sym(self.get_point_position_name(), 3) return cs.Function( "planar_terrain_orientation", [point_position], [cs.MX.eye(3)], - [self._point_position_name], + [self.get_point_position_name()], ["plane_rotation"], self._options, ) From 588dda5a03aba10f7ae690decd9ff1870e4f3aa5 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 12:32:39 +0200 Subject: [PATCH 025/106] Added friction expression to planner and fixed the use of dcc expressions --- src/hippopt/robot_planning/expressions/contacts.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index b57041b8..735f1b54 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -70,7 +70,7 @@ def friction_cone_square_margin( [point_position, point_force, static_friction], [margin], [point_position_name, point_force_name, static_friction_name], - ["margin"], + ["friction_cone_square_margin"], options, ) From c11705dec5cf7b13554984413fff182ba3786369 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 8 Aug 2023 19:11:23 +0200 Subject: [PATCH 026/106] Added additional constraint for the maximum feet relative height --- .../robot_planning/expressions/contacts.py | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index 735f1b54..988be458 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -8,7 +8,7 @@ def normal_force_component( point_position_name: str = None, point_force_name: str = "point_force", options: dict = None, - **_ + **_, ) -> cs.Function: options = {} if options is None else options point_position_name = ( @@ -39,7 +39,7 @@ def friction_cone_square_margin( point_force_name: str = "point_force", static_friction_name: str = "mu_s", options: dict = None, - **_ + **_, ) -> cs.Function: options = {} if options is None else options point_position_name = ( @@ -76,14 +76,24 @@ def friction_cone_square_margin( def contact_points_centroid( - number_of_points: int, point_position_name: str = "p_#", options: dict = None, **_ + number_of_points: int, + point_position_names: list[str] = None, + options: dict = None, + **_, ) -> cs.Function: options = {} if options is None else options + if point_position_names is None: + point_position_names = [] + for i in range(number_of_points): + point_position_names.append(f"p{i}") + + assert len(point_position_names) == number_of_points + input_vars = [] p = [] for i in range(number_of_points): - p.append(cs.MX.sym(point_position_name.replace("#", str(i)), 3)) + p.append(cs.MX.sym(point_position_names[i], 3)) input_vars.append(p[i]) input_names = [] @@ -113,7 +123,7 @@ def contact_points_yaw_alignment( second_point_name: str = "p_1", desired_yaw_name: str = "desired_yaw", options: dict = None, - **_ + **_, ) -> cs.Function: options = {} if options is None else options @@ -139,7 +149,7 @@ def swing_height_heuristic( point_velocity_name: str = "p_dot", desired_height_name: str = "h_desired", options: dict = None, - **_ + **_, ) -> cs.Function: options = {} if options is None else options From b9364eae82d984ea2d6e29c98da84c16738ae4e1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 13:11:43 +0200 Subject: [PATCH 027/106] Separated quaternion error function --- src/hippopt/robot_planning/__init__.py | 3 +- .../robot_planning/expressions/kinematics.py | 14 ++++---- .../robot_planning/utilities/quaternion.py | 34 +++++++++++++++++++ 3 files changed, 42 insertions(+), 9 deletions(-) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 28a5d941..3f85f801 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -19,9 +19,10 @@ centroidal_momentum_from_kinematics, frames_relative_position, point_position_from_kinematics, - quaternion_error, + quaternion_error_from_kinematics, ) from .utilities.quaternion import ( + quaternion_xyzw_error, quaternion_xyzw_normalization, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index ad648663..1fd7d87b 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -3,6 +3,7 @@ from adam.casadi import KinDynComputations from hippopt.robot_planning.utilities.quaternion import ( + quaternion_xyzw_error, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) @@ -200,7 +201,7 @@ def frames_relative_position( ) -def quaternion_error( +def quaternion_error_from_kinematics( kindyn_object: KinDynComputations, target_frame: str, base_position_name: str = "base_position", @@ -223,16 +224,13 @@ def quaternion_error( target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) target_pose = target_fk_function(base_pose, joint_positions) - target_rotation = liecasadi.SO3.from_matrix(target_pose[:3, :3]) - desired_rotation = liecasadi.SO3.from_quat(desired_quaternion) + target_quaternion = liecasadi.SO3.from_matrix(target_pose[:3, :3]).as_quat() + quaternion_error_fun = quaternion_xyzw_error(options=options) - rotation_error = desired_rotation.inverse() * target_rotation - identity = liecasadi.SO3.Identity() - - error = rotation_error.as_quat() - identity.as_quat() + error = quaternion_error_fun(target_quaternion, desired_quaternion) return cs.Function( - "quaternion_error", + "quaternion_error_from_kinematics", [ base_position, base_quaternion, diff --git a/src/hippopt/robot_planning/utilities/quaternion.py b/src/hippopt/robot_planning/utilities/quaternion.py index e62db760..2f304a2a 100644 --- a/src/hippopt/robot_planning/utilities/quaternion.py +++ b/src/hippopt/robot_planning/utilities/quaternion.py @@ -49,3 +49,37 @@ def quaternion_xyzw_velocity_to_right_trivialized_angular_velocity( ["right_trivialized_angular_velocity"], options, ) + + +def quaternion_xyzw_error( + quaternion_xyzw_name: str = "quaternion", + desired_quaternion_xyzw_name: str = "desired_quaternion", + options: dict = None, + **_, +): + options = {} if options is None else options + quaternion = cs.MX.sym(quaternion_xyzw_name, 4) + desired_quaternion = cs.MX.sym(desired_quaternion_xyzw_name, 4) + + target_rotation = liecasadi.SO3.from_quat(quaternion) + desired_rotation = liecasadi.SO3.from_quat(desired_quaternion) + + rotation_error = desired_rotation.inverse() * target_rotation + identity = liecasadi.SO3.Identity() + + error = rotation_error.as_quat() - identity.as_quat() + + return cs.Function( + "quaternion_error", + [ + quaternion, + desired_quaternion, + ], + [error], + [ + quaternion_xyzw_name, + desired_quaternion_xyzw_name, + ], + ["quaternion_error"], + options, + ) From 84a377dce6968cbae6f18ab6cb4ebccbef05c80b Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 10 Aug 2023 19:04:50 +0200 Subject: [PATCH 028/106] Moved feet references to custom class and implemented yaw regularization task Renamed the yaw alignment function for consistency --- src/hippopt/robot_planning/__init__.py | 2 +- src/hippopt/robot_planning/expressions/contacts.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 3f85f801..57b060d3 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -9,7 +9,7 @@ ) from .expressions.contacts import ( contact_points_centroid, - contact_points_yaw_alignment, + contact_points_yaw_alignment_error, friction_cone_square_margin, normal_force_component, swing_height_heuristic, diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index 988be458..ace2bee5 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -118,7 +118,7 @@ def contact_points_centroid( ) -def contact_points_yaw_alignment( +def contact_points_yaw_alignment_error( first_point_name: str = "p_0", second_point_name: str = "p_1", desired_yaw_name: str = "desired_yaw", @@ -134,11 +134,11 @@ def contact_points_yaw_alignment( yaw_alignment = cs.horzcat([-cs.sin(yaw), cs.cos(yaw)]) @ (p1 - p0)[:2] return cs.Function( - "contact_points_yaw_alignment", + "contact_points_yaw_alignment_error", [p0, p1, yaw], [yaw_alignment], [first_point_name, second_point_name, desired_yaw_name], - ["yaw_alignment"], + ["yaw_alignment_error"], options, ) From 2673c1425d64b7dc42fcedcfa721bdce1d4fd741 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 11:48:51 +0200 Subject: [PATCH 029/106] Added contact point regularization --- src/hippopt/robot_planning/expressions/contacts.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index ace2bee5..c4458c80 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -145,7 +145,7 @@ def contact_points_yaw_alignment_error( def swing_height_heuristic( terrain: TerrainDescriptor, - point_name: str = "p", + point_position_name: str = "p", point_velocity_name: str = "p_dot", desired_height_name: str = "h_desired", options: dict = None, @@ -153,7 +153,7 @@ def swing_height_heuristic( ) -> cs.Function: options = {} if options is None else options - point = cs.MX.sym(point_name, 3) + point = cs.MX.sym(point_position_name, 3) point_velocity = cs.MX.sym(point_velocity_name, 3) desired_height = cs.MX.sym(desired_height_name, 1) @@ -171,7 +171,7 @@ def swing_height_heuristic( "swing_height_heuristic", [point, point_velocity, desired_height], [heuristic], - [point_name, point_velocity_name, desired_height_name], + [point_position_name, point_velocity_name, desired_height_name], ["heuristic"], options, ) From 3b1f3bc0407f42bf2afac4af161db536a84f2baf Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 11 Aug 2023 18:28:43 +0200 Subject: [PATCH 030/106] Refactor variables to extract initial states --- src/hippopt/robot_planning/__init__.py | 14 +++- .../robot_planning/variables/contacts.py | 35 +++++---- .../robot_planning/variables/floating_base.py | 72 ++++++++++++++----- 3 files changed, 89 insertions(+), 32 deletions(-) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 57b060d3..60ae9b35 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -27,9 +27,21 @@ quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor -from .variables.contacts import ContactPoint, ContactPointDescriptor +from .variables.contacts import ( + ContactPointDescriptor, + ContactPointState, + ContactPointStateDerivative, + FeetContactPointDescriptors, + FeetContactPoints, +) from .variables.floating_base import ( FloatingBaseSystem, + FloatingBaseSystemState, + FloatingBaseSystemStateDerivative, FreeFloatingObject, + FreeFloatingObjectState, + FreeFloatingObjectStateDerivative, KinematicTree, + KinematicTreeState, + KinematicTreeStateDerivative, ) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 50abd513..a0462778 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -30,16 +30,9 @@ def __post_init__( @dataclasses.dataclass -class ContactPoint(OptimizationObject): +class ContactPointState(OptimizationObject): p: StorageType = default_storage_field(Variable) - v: StorageType = default_storage_field(Variable) f: StorageType = default_storage_field(Variable) - f_dot: StorageType = default_storage_field(Variable) - - # Initial conditions - p0: StorageType = default_storage_field(Parameter) - v0: StorageType = default_storage_field(Parameter) - f0: StorageType = default_storage_field(Parameter) descriptor: ContactPointDescriptor = default_composite_field(time_varying=False) @@ -49,12 +42,28 @@ class ContactPoint(OptimizationObject): def __post_init__(self, input_descriptor: ContactPointDescriptor) -> None: self.p = np.zeros(3) - self.v = np.zeros(3) self.f = np.zeros(3) + + self.descriptor = input_descriptor + + +@dataclasses.dataclass +class ContactPointStateDerivative(OptimizationObject): + v: StorageType = default_storage_field(Variable) + f_dot: StorageType = default_storage_field(Variable) + + def __post_init__(self) -> None: + self.v = np.zeros(3) self.f_dot = np.zeros(3) - self.p0 = np.zeros(3) - self.v0 = np.zeros(3) - self.f0 = np.zeros(3) - self.descriptor = input_descriptor +@dataclasses.dataclass +class FeetContactPointDescriptors: + left: list[ContactPointDescriptor] = dataclasses.field(default_factory=list) + right: list[ContactPointDescriptor] = dataclasses.field(default_factory=list) + + +@dataclasses.dataclass +class FeetContactPoints(OptimizationObject): + left: list[ContactPointState] = default_composite_field() + right: list[ContactPointState] = default_composite_field() diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index f99b9c34..26ece923 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -4,7 +4,6 @@ from hippopt import ( OptimizationObject, - Parameter, StorageType, Variable, default_composite_field, @@ -13,43 +12,80 @@ @dataclasses.dataclass -class FreeFloatingObject(OptimizationObject): +class FreeFloatingObjectState(OptimizationObject): position: StorageType = default_storage_field(Variable) - linear_velocity: StorageType = default_storage_field(Variable) quaternion_xyzw: StorageType = default_storage_field(Variable) - quaternion_velocity_xyzw: StorageType = default_storage_field(Variable) - - # Initial conditions - initial_position: StorageType = default_storage_field(Parameter) - initial_quaternion_xyzw: StorageType = default_storage_field(Parameter) def __post_init__(self): self.position = np.zeros(3) - self.linear_velocity = np.zeros(3) self.quaternion_xyzw = np.zeros(4) self.quaternion_xyzw[3] = 1.0 + + +@dataclasses.dataclass +class FreeFloatingObjectStateDerivative(OptimizationObject): + linear_velocity: StorageType = default_storage_field(Variable) + quaternion_velocity_xyzw: StorageType = default_storage_field(Variable) + + def __post_init__(self): + self.linear_velocity = np.zeros(3) self.quaternion_velocity_xyzw = np.zeros(4) - self.initial_position = np.zeros(3) - self.initial_quaternion_xyzw = np.zeros(4) - self.initial_quaternion_xyzw[3] = 1.0 + +@dataclasses.dataclass +class FreeFloatingObject(FreeFloatingObjectState, FreeFloatingObjectStateDerivative): + pass @dataclasses.dataclass -class KinematicTree(OptimizationObject): +class KinematicTreeState(OptimizationObject): positions: StorageType = default_storage_field(Variable) - velocities: StorageType = default_storage_field(Variable) - - # Initial conditions - initial_positions: StorageType = default_storage_field(Parameter) number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_joints: int): self.positions = np.zeros(number_of_joints) + + +@dataclasses.dataclass +class KinematicTreeStateDerivative(OptimizationObject): + velocities: StorageType = default_storage_field(Variable) + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_joints: int): self.velocities = np.zeros(number_of_joints) - self.initial_positions = np.zeros(number_of_joints) + +@dataclasses.dataclass +class KinematicTree(KinematicTreeState, KinematicTreeStateDerivative): + pass + + +@dataclasses.dataclass +class FloatingBaseSystemState(OptimizationObject): + base: FreeFloatingObjectState = default_composite_field( + factory=FreeFloatingObjectState + ) + joints: KinematicTreeState = default_composite_field() + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_joints: int): + self.joints = KinematicTreeState(number_of_joints=number_of_joints) + + +@dataclasses.dataclass +class FloatingBaseSystemStateDerivative(OptimizationObject): + base: FreeFloatingObjectStateDerivative = default_composite_field( + factory=FreeFloatingObjectStateDerivative + ) + joints: KinematicTreeStateDerivative = default_composite_field() + + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__(self, number_of_joints: int): + self.joints = KinematicTreeStateDerivative(number_of_joints=number_of_joints) @dataclasses.dataclass From 6ffdf9e37585e642fe8bc3d470d6e49ad45a218f Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 14:33:37 +0200 Subject: [PATCH 031/106] Improved definition of composite types Also, making sure to call the superclass __post_init__ --- .../robot_planning/variables/contacts.py | 5 +- .../robot_planning/variables/floating_base.py | 78 ++++++++++++++----- 2 files changed, 61 insertions(+), 22 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index a0462778..a611c261 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -3,6 +3,7 @@ import numpy as np from hippopt import ( + CompositeType, OptimizationObject, Parameter, StorageType, @@ -34,7 +35,9 @@ class ContactPointState(OptimizationObject): p: StorageType = default_storage_field(Variable) f: StorageType = default_storage_field(Variable) - descriptor: ContactPointDescriptor = default_composite_field(time_varying=False) + descriptor: CompositeType[ContactPointDescriptor] = default_composite_field( + factory=ContactPointDescriptor, time_varying=False + ) input_descriptor: dataclasses.InitVar[ContactPointDescriptor] = dataclasses.field( default=None diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index 26ece923..1318962f 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -3,6 +3,7 @@ import numpy as np from hippopt import ( + CompositeType, OptimizationObject, StorageType, Variable, @@ -34,66 +35,101 @@ def __post_init__(self): @dataclasses.dataclass class FreeFloatingObject(FreeFloatingObjectState, FreeFloatingObjectStateDerivative): - pass + def __post_init__(self): + FreeFloatingObjectState.__post_init__(self) + FreeFloatingObjectStateDerivative.__post_init__(self) @dataclasses.dataclass class KinematicTreeState(OptimizationObject): positions: StorageType = default_storage_field(Variable) - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=None) - def __post_init__(self, number_of_joints: int): - self.positions = np.zeros(number_of_joints) + def __post_init__(self, number_of_joints_state: int): + self.positions = np.zeros(number_of_joints_state) @dataclasses.dataclass class KinematicTreeStateDerivative(OptimizationObject): velocities: StorageType = default_storage_field(Variable) - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_joints_derivative: dataclasses.InitVar[int] = dataclasses.field( + default=None + ) - def __post_init__(self, number_of_joints: int): - self.velocities = np.zeros(number_of_joints) + def __post_init__(self, number_of_joints_derivative: int): + self.velocities = np.zeros(number_of_joints_derivative) @dataclasses.dataclass class KinematicTree(KinematicTreeState, KinematicTreeStateDerivative): - pass + def __post_init__( + self, + number_of_joints_derivative: int = None, + number_of_joints_state: int = None, + ): + if number_of_joints_derivative is None and number_of_joints_state is None: + raise ValueError( + "Either number_of_joints_derivative or " + "number_of_joints_state must be specified" + ) + number_of_joints_state = ( + number_of_joints_derivative + if number_of_joints_state is None + else number_of_joints_state + ) + number_of_joints_derivative = ( + number_of_joints_state + if number_of_joints_derivative is None + else number_of_joints_derivative + ) + KinematicTreeState.__post_init__( + self, number_of_joints_state=number_of_joints_state + ) + KinematicTreeStateDerivative.__post_init__( + self, number_of_joints_derivative=number_of_joints_derivative + ) @dataclasses.dataclass class FloatingBaseSystemState(OptimizationObject): - base: FreeFloatingObjectState = default_composite_field( + base: CompositeType[FreeFloatingObjectState] = default_composite_field( factory=FreeFloatingObjectState ) - joints: KinematicTreeState = default_composite_field() + joints: CompositeType[KinematicTreeState] = default_composite_field() - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=None) - def __post_init__(self, number_of_joints: int): - self.joints = KinematicTreeState(number_of_joints=number_of_joints) + def __post_init__(self, number_of_joints_state: int): + self.joints = KinematicTreeState(number_of_joints_state=number_of_joints_state) @dataclasses.dataclass class FloatingBaseSystemStateDerivative(OptimizationObject): - base: FreeFloatingObjectStateDerivative = default_composite_field( + base: CompositeType[FreeFloatingObjectStateDerivative] = default_composite_field( factory=FreeFloatingObjectStateDerivative ) - joints: KinematicTreeStateDerivative = default_composite_field() + joints: CompositeType[KinematicTreeStateDerivative] = default_composite_field() - number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_joints_derivative: dataclasses.InitVar[int] = dataclasses.field( + default=None + ) - def __post_init__(self, number_of_joints: int): - self.joints = KinematicTreeStateDerivative(number_of_joints=number_of_joints) + def __post_init__(self, number_of_joints_derivative: int): + self.joints = KinematicTreeStateDerivative( + number_of_joints_derivative=number_of_joints_derivative + ) @dataclasses.dataclass class FloatingBaseSystem(OptimizationObject): - base: FreeFloatingObject = default_composite_field(factory=FreeFloatingObject) - joints: KinematicTree = default_composite_field() + base: CompositeType[FreeFloatingObject] = default_composite_field( + factory=FreeFloatingObject + ) + joints: CompositeType[KinematicTree] = default_composite_field() number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_joints: int): - self.joints = KinematicTree(number_of_joints=number_of_joints) + self.joints = KinematicTree(number_of_joints_state=number_of_joints) From 217e041cc26ab6fbe342579c6728c8e5ff822c51 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 16:13:44 +0200 Subject: [PATCH 032/106] Added method to generate the list of contact point descriptor for a rectangular foot --- .../robot_planning/variables/contacts.py | 29 +++++++++++++++++++ 1 file changed, 29 insertions(+) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index a611c261..73c8c2a3 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -29,6 +29,35 @@ def __post_init__( self.foot_frame = input_foot_frame self.position_in_foot_frame = input_position_in_foot_frame + @staticmethod + def rectangular_foot( + foot_frame: str, + x_length: float, + y_length: float, + top_left_point_position: np.array, + ): + return [ + ContactPointDescriptor( + input_foot_frame=foot_frame, + input_position_in_foot_frame=top_left_point_position, + ), + ContactPointDescriptor( + input_foot_frame=foot_frame, + input_position_in_foot_frame=top_left_point_position + + np.array([-x_length, 0.0, 0.0]), + ), + ContactPointDescriptor( + input_foot_frame=foot_frame, + input_position_in_foot_frame=top_left_point_position + + np.array([-x_length, -y_length, 0.0]), + ), + ContactPointDescriptor( + input_foot_frame=foot_frame, + input_position_in_foot_frame=top_left_point_position + + np.array([0.0, -y_length, 0.0]), + ), + ] + @dataclasses.dataclass class ContactPointState(OptimizationObject): From cff0948e9ce8b27a871c64c88d4eb43effd5a7bf Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 16 Aug 2023 19:10:27 +0200 Subject: [PATCH 033/106] Fixed initialization of composite variables --- .../robot_planning/variables/contacts.py | 4 ++-- .../robot_planning/variables/floating_base.py | 21 ++++++++++++------- 2 files changed, 15 insertions(+), 10 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 73c8c2a3..0b0b9176 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -97,5 +97,5 @@ class FeetContactPointDescriptors: @dataclasses.dataclass class FeetContactPoints(OptimizationObject): - left: list[ContactPointState] = default_composite_field() - right: list[ContactPointState] = default_composite_field() + left: list[ContactPointState] = default_composite_field(factory=list) + right: list[ContactPointState] = default_composite_field(factory=list) diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index 1318962f..52316621 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -44,7 +44,7 @@ def __post_init__(self): class KinematicTreeState(OptimizationObject): positions: StorageType = default_storage_field(Variable) - number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=None) + number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=0) def __post_init__(self, number_of_joints_state: int): self.positions = np.zeros(number_of_joints_state) @@ -70,10 +70,9 @@ def __post_init__( number_of_joints_state: int = None, ): if number_of_joints_derivative is None and number_of_joints_state is None: - raise ValueError( - "Either number_of_joints_derivative or " - "number_of_joints_state must be specified" - ) + number_of_joints_state = 0 + number_of_joints_derivative = 0 + number_of_joints_state = ( number_of_joints_derivative if number_of_joints_state is None @@ -97,7 +96,9 @@ class FloatingBaseSystemState(OptimizationObject): base: CompositeType[FreeFloatingObjectState] = default_composite_field( factory=FreeFloatingObjectState ) - joints: CompositeType[KinematicTreeState] = default_composite_field() + joints: CompositeType[KinematicTreeState] = default_composite_field( + factory=KinematicTreeState + ) number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=None) @@ -110,7 +111,9 @@ class FloatingBaseSystemStateDerivative(OptimizationObject): base: CompositeType[FreeFloatingObjectStateDerivative] = default_composite_field( factory=FreeFloatingObjectStateDerivative ) - joints: CompositeType[KinematicTreeStateDerivative] = default_composite_field() + joints: CompositeType[KinematicTreeStateDerivative] = default_composite_field( + factory=KinematicTreeStateDerivative + ) number_of_joints_derivative: dataclasses.InitVar[int] = dataclasses.field( default=None @@ -127,7 +130,9 @@ class FloatingBaseSystem(OptimizationObject): base: CompositeType[FreeFloatingObject] = default_composite_field( factory=FreeFloatingObject ) - joints: CompositeType[KinematicTree] = default_composite_field() + joints: CompositeType[KinematicTree] = default_composite_field( + factory=KinematicTree + ) number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) From 998f5562a45b395cff61201aee2b9f02984814cc Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 17 Aug 2023 12:49:34 +0200 Subject: [PATCH 034/106] Fixed used of casadi transpose and horzcat --- src/hippopt/robot_planning/expressions/complementarity.py | 8 ++++---- src/hippopt/robot_planning/expressions/contacts.py | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/complementarity.py b/src/hippopt/robot_planning/expressions/complementarity.py index 3946df49..04d004a1 100644 --- a/src/hippopt/robot_planning/expressions/complementarity.py +++ b/src/hippopt/robot_planning/expressions/complementarity.py @@ -28,7 +28,7 @@ def dcc_planar_complementarity( terrain_orientation = terrain_orientation_fun(point_position) planar_multiplier = cs.tanh(height_multiplier * height) - multipliers = cs.diag(cs.horzcat([planar_multiplier, planar_multiplier, 1])) + multipliers = cs.diag(cs.horzcat(planar_multiplier, planar_multiplier, 1)) planar_complementarity = terrain_orientation @ multipliers @ point_control return cs.Function( @@ -74,13 +74,13 @@ def dcc_complementarity_margin( # See Sec III.A of https://ieeexplore.ieee.org/abstract/document/9847574 height_derivative = cs.jtimes(height, point_position, point_velocity) normal_derivative = cs.jtimes(normal_direction, point_position, point_velocity) - normal_force = normal_direction.T() @ point_force - normal_force_derivative = normal_direction.T() @ point_force_derivative + normal_force = normal_direction.T @ point_force + normal_force_derivative = normal_direction.T @ point_force_derivative complementarity = height * normal_force csi = ( height_derivative * normal_force - + height * point_force.T() @ normal_derivative + + height * point_force.T @ normal_derivative + height * normal_force_derivative ) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index c4458c80..9a2bb628 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -21,7 +21,7 @@ def normal_force_component( normal_direction_fun = terrain.normal_direction_function() - normal_component = normal_direction_fun(point_position).T() @ point_force + normal_component = normal_direction_fun(point_position).T @ point_force return cs.Function( "normal_force_component", @@ -53,7 +53,7 @@ def friction_cone_square_margin( orientation_fun = terrain.orientation_function() terrain_orientation = orientation_fun(point_position) - force_in_contact = terrain_orientation.T() @ point_force + force_in_contact = terrain_orientation.T @ point_force # In principle, it should be sqrt(fx^2 + fy^2) <= u * fz, # but since both sides are positive, we square them both. @@ -61,7 +61,7 @@ def friction_cone_square_margin( # (u * fz)^2 - (fx^2 + fy^2) >= 0 # that is equal to # [-1, -1, u^2] * f.^2 - margin = cs.horzcat([-1, -1, cs.constpow(static_friction, 2)]) * cs.constpow( + margin = cs.horzcat(-1, -1, cs.constpow(static_friction, 2)) @ cs.constpow( force_in_contact, 2 ) @@ -163,7 +163,7 @@ def swing_height_heuristic( terrain_orientation = terrain_orientation_fun(point) height_difference = terrain_height - desired_height - planar_velocity = (terrain_orientation.T() @ point_velocity)[:2] + planar_velocity = (terrain_orientation.T @ point_velocity)[:2] heuristic = 0.5 * (cs.constpow(height_difference, 2) + cs.sumsqr(planar_velocity)) From d451dfadfcc75dfa0b6ededbe4810d3708d1ee07 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 17 Aug 2023 16:32:00 +0200 Subject: [PATCH 035/106] Multiple bugfixes on expressions - Wrong use of horzcat - Missed the use of a label - Wrong generation of functions --- src/hippopt/robot_planning/expressions/contacts.py | 2 +- src/hippopt/robot_planning/expressions/kinematics.py | 7 ++++--- src/hippopt/robot_planning/utilities/quaternion.py | 6 +++--- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index 9a2bb628..2d7f3840 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -131,7 +131,7 @@ def contact_points_yaw_alignment_error( p1 = cs.MX.sym(second_point_name, 3) yaw = cs.MX.sym(desired_yaw_name, 1) - yaw_alignment = cs.horzcat([-cs.sin(yaw), cs.cos(yaw)]) @ (p1 - p0)[:2] + yaw_alignment = cs.horzcat(-cs.sin(yaw), cs.cos(yaw)) @ (p1 - p0)[:2] return cs.Function( "contact_points_yaw_alignment_error", diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index 1fd7d87b..e3a84ef7 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -37,7 +37,7 @@ def centroidal_momentum_from_kinematics( base_angular_velocity_fun = ( quaternion_xyzw_velocity_to_right_trivialized_angular_velocity( quaternion_xyzw_name=base_quaternion_xyzw_name, - base_quaternion_xyzw_derivative_name=base_quaternion_xyzw_derivative_name, + quaternion_xyzw_velocity_name=base_quaternion_xyzw_derivative_name, options=options, ) ) @@ -46,7 +46,7 @@ def centroidal_momentum_from_kinematics( base_quaternion_xyzw_name: base_quaternion, base_quaternion_xyzw_derivative_name: base_quaternion_derivative, } - ) + )["right_trivialized_angular_velocity"] robot_velocity = cs.vertcat( base_position_derivative, base_angular_velocity, joint_velocities @@ -196,7 +196,8 @@ def frames_relative_position( "frames_relative_position", [joint_positions], [relative_position], - "relative_position", + [joint_positions_name], + ["relative_position"], options, ) diff --git a/src/hippopt/robot_planning/utilities/quaternion.py b/src/hippopt/robot_planning/utilities/quaternion.py index 2f304a2a..82ee1c43 100644 --- a/src/hippopt/robot_planning/utilities/quaternion.py +++ b/src/hippopt/robot_planning/utilities/quaternion.py @@ -33,13 +33,13 @@ def quaternion_xyzw_velocity_to_right_trivialized_angular_velocity( quaternion_velocity = cs.MX.sym(quaternion_xyzw_velocity_name, 4) q_w = quaternion[3] - q_i = quaternion[1:3] + q_i = quaternion[:3] q_dot_w = quaternion_velocity[3] - q_dot_i = quaternion_velocity[1:3] + q_dot_i = quaternion_velocity[:3] # See Sec. 1.5.3 of https://arxiv.org/pdf/0811.2889.pdf - angular_velocity = 2 * (-q_dot_w * q_i + q_w * q_dot_i - q_dot_i.cross(q_i)) + angular_velocity = 2 * (-q_dot_w * q_i + q_w * q_dot_i - cs.cross(q_dot_i, q_i)) return cs.Function( "quaternion_xyzw_velocity_to_right_trivialized_angular_velocity", From 97d5de72bd566da526336fa1da3bf8a4f1274e16 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 12 Sep 2023 19:53:18 +0200 Subject: [PATCH 036/106] Exploiting the OverridableVariable mechanism and added HumanoidState --- src/hippopt/robot_planning/__init__.py | 3 +- .../robot_planning/variables/__init__.py | 2 +- .../robot_planning/variables/contacts.py | 10 ++-- .../robot_planning/variables/floating_base.py | 14 ++--- .../robot_planning/variables/humanoid.py | 58 +++++++++++++++++++ 5 files changed, 73 insertions(+), 14 deletions(-) create mode 100644 src/hippopt/robot_planning/variables/humanoid.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 60ae9b35..374c07e4 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -1,4 +1,4 @@ -from . import dynamics, expressions, utilities +from . import dynamics, expressions, utilities, variables from .dynamics.centroidal import ( centroidal_dynamics_with_point_forces, com_dynamics_from_momentum, @@ -45,3 +45,4 @@ KinematicTreeState, KinematicTreeStateDerivative, ) +from .variables.humanoid import HumanoidState diff --git a/src/hippopt/robot_planning/variables/__init__.py b/src/hippopt/robot_planning/variables/__init__.py index 30640706..aaa3d8a8 100644 --- a/src/hippopt/robot_planning/variables/__init__.py +++ b/src/hippopt/robot_planning/variables/__init__.py @@ -1 +1 @@ -from . import contacts, floating_base +from . import contacts, floating_base, humanoid diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 0b0b9176..2ccd5be1 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -5,9 +5,9 @@ from hippopt import ( CompositeType, OptimizationObject, + OverridableVariable, Parameter, StorageType, - Variable, default_composite_field, default_storage_field, ) @@ -61,8 +61,8 @@ def rectangular_foot( @dataclasses.dataclass class ContactPointState(OptimizationObject): - p: StorageType = default_storage_field(Variable) - f: StorageType = default_storage_field(Variable) + p: StorageType = default_storage_field(OverridableVariable) + f: StorageType = default_storage_field(OverridableVariable) descriptor: CompositeType[ContactPointDescriptor] = default_composite_field( factory=ContactPointDescriptor, time_varying=False @@ -81,8 +81,8 @@ def __post_init__(self, input_descriptor: ContactPointDescriptor) -> None: @dataclasses.dataclass class ContactPointStateDerivative(OptimizationObject): - v: StorageType = default_storage_field(Variable) - f_dot: StorageType = default_storage_field(Variable) + v: StorageType = default_storage_field(OverridableVariable) + f_dot: StorageType = default_storage_field(OverridableVariable) def __post_init__(self) -> None: self.v = np.zeros(3) diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index 52316621..e86e6b65 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -5,8 +5,8 @@ from hippopt import ( CompositeType, OptimizationObject, + OverridableVariable, StorageType, - Variable, default_composite_field, default_storage_field, ) @@ -14,8 +14,8 @@ @dataclasses.dataclass class FreeFloatingObjectState(OptimizationObject): - position: StorageType = default_storage_field(Variable) - quaternion_xyzw: StorageType = default_storage_field(Variable) + position: StorageType = default_storage_field(OverridableVariable) + quaternion_xyzw: StorageType = default_storage_field(OverridableVariable) def __post_init__(self): self.position = np.zeros(3) @@ -25,8 +25,8 @@ def __post_init__(self): @dataclasses.dataclass class FreeFloatingObjectStateDerivative(OptimizationObject): - linear_velocity: StorageType = default_storage_field(Variable) - quaternion_velocity_xyzw: StorageType = default_storage_field(Variable) + linear_velocity: StorageType = default_storage_field(OverridableVariable) + quaternion_velocity_xyzw: StorageType = default_storage_field(OverridableVariable) def __post_init__(self): self.linear_velocity = np.zeros(3) @@ -42,7 +42,7 @@ def __post_init__(self): @dataclasses.dataclass class KinematicTreeState(OptimizationObject): - positions: StorageType = default_storage_field(Variable) + positions: StorageType = default_storage_field(OverridableVariable) number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=0) @@ -52,7 +52,7 @@ def __post_init__(self, number_of_joints_state: int): @dataclasses.dataclass class KinematicTreeStateDerivative(OptimizationObject): - velocities: StorageType = default_storage_field(Variable) + velocities: StorageType = default_storage_field(OverridableVariable) number_of_joints_derivative: dataclasses.InitVar[int] = dataclasses.field( default=None diff --git a/src/hippopt/robot_planning/variables/humanoid.py b/src/hippopt/robot_planning/variables/humanoid.py new file mode 100644 index 00000000..f2dbc3f8 --- /dev/null +++ b/src/hippopt/robot_planning/variables/humanoid.py @@ -0,0 +1,58 @@ +import dataclasses + +import numpy as np + +from hippopt import ( + CompositeType, + OptimizationObject, + OverridableVariable, + StorageType, + default_composite_field, + default_storage_field, +) +from hippopt.robot_planning.variables.contacts import ( + ContactPointState, + FeetContactPointDescriptors, + FeetContactPoints, +) +from hippopt.robot_planning.variables.floating_base import FloatingBaseSystemState + + +@dataclasses.dataclass +class HumanoidState(OptimizationObject): + contact_points: CompositeType[FeetContactPoints] = default_composite_field( + factory=FeetContactPoints, time_varying=False + ) + + kinematics: CompositeType[FloatingBaseSystemState] = default_composite_field( + factory=FloatingBaseSystemState, time_varying=False + ) + + com: StorageType = default_storage_field(OverridableVariable) + centroidal_momentum: StorageType = default_storage_field(OverridableVariable) + + contact_point_descriptors: dataclasses.InitVar[ + FeetContactPointDescriptors + ] = dataclasses.field(default=None) + number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) + + def __post_init__( + self, + contact_point_descriptors: FeetContactPointDescriptors, + number_of_joints: int, + ) -> None: + if contact_point_descriptors is not None: + self.contact_points.left = [ + ContactPointState(descriptor=point) + for point in contact_point_descriptors.left + ] + self.contact_points.right = [ + ContactPointState(descriptor=point) + for point in contact_point_descriptors.right + ] + number_of_joints = number_of_joints if number_of_joints is not None else 0 + self.kinematics = FloatingBaseSystemState( + number_of_joints_state=number_of_joints + ) + self.com = np.zeros(3) + self.centroidal_momentum = np.zeros(6) From 218beb23afcd25883ca3184da1a0e82a9c90e545 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 11:42:51 +0200 Subject: [PATCH 037/106] Added relaxed complementarity margin --- src/hippopt/robot_planning/__init__.py | 1 + .../expressions/complementarity.py | 45 +++++++++++++++++++ 2 files changed, 46 insertions(+) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 374c07e4..c01fc8b8 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -6,6 +6,7 @@ from .expressions.complementarity import ( dcc_complementarity_margin, dcc_planar_complementarity, + relaxed_complementarity_margin, ) from .expressions.contacts import ( contact_points_centroid, diff --git a/src/hippopt/robot_planning/expressions/complementarity.py b/src/hippopt/robot_planning/expressions/complementarity.py index 04d004a1..591f50b0 100644 --- a/src/hippopt/robot_planning/expressions/complementarity.py +++ b/src/hippopt/robot_planning/expressions/complementarity.py @@ -108,3 +108,48 @@ def dcc_complementarity_margin( ["dcc_complementarity_margin"], options, ) + + +def relaxed_complementarity_margin( + terrain: TerrainDescriptor, + point_position_name: str = None, + point_force_name: str = "point_force", + relaxed_complementarity_epsilon_name: str = "eps", + options: dict = None, + **_ +) -> cs.Function: + options = {} if options is None else options + point_position_name = ( + terrain.get_point_position_name() + if point_position_name is None + else point_position_name + ) + point_position = cs.MX.sym(point_position_name, 3) + point_force = cs.MX.sym(point_force_name, 3) + eps = cs.MX.sym(relaxed_complementarity_epsilon_name, 1) + + normal_direction_fun = terrain.normal_direction_function() + height_function = terrain.height_function() + + height = height_function(point_position) + normal_direction = normal_direction_fun(point_position) + normal_force = normal_direction.T @ point_force + + margin = eps - height * normal_force + + return cs.Function( + "relaxed_complementarity_margin", + [ + point_position, + point_force, + eps, + ], + [margin], + [ + point_position_name, + point_force_name, + relaxed_complementarity_epsilon_name, + ], + ["relaxed_complementarity_margin"], + options, + ) From acd95cbe6df3b46062448d9a68048dfb7a03b530 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 17:29:28 +0200 Subject: [PATCH 038/106] Fixed bug in Humanoid state --- src/hippopt/robot_planning/variables/humanoid.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/hippopt/robot_planning/variables/humanoid.py b/src/hippopt/robot_planning/variables/humanoid.py index f2dbc3f8..60db2a8b 100644 --- a/src/hippopt/robot_planning/variables/humanoid.py +++ b/src/hippopt/robot_planning/variables/humanoid.py @@ -43,11 +43,11 @@ def __post_init__( ) -> None: if contact_point_descriptors is not None: self.contact_points.left = [ - ContactPointState(descriptor=point) + ContactPointState(input_descriptor=point) for point in contact_point_descriptors.left ] self.contact_points.right = [ - ContactPointState(descriptor=point) + ContactPointState(input_descriptor=point) for point in contact_point_descriptors.right ] number_of_joints = number_of_joints if number_of_joints is not None else 0 From 489288177bb22677564507f33a4d6f28ccc9aa72 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 17:32:39 +0200 Subject: [PATCH 039/106] Moved centroidal and quaternion expressions to the expressions folder --- src/hippopt/robot_planning/__init__.py | 6 +++--- src/hippopt/robot_planning/dynamics/__init__.py | 1 - .../robot_planning/{dynamics => expressions}/centroidal.py | 0 src/hippopt/robot_planning/expressions/kinematics.py | 2 +- .../robot_planning/{utilities => expressions}/quaternion.py | 0 src/hippopt/robot_planning/utilities/__init__.py | 2 +- 6 files changed, 5 insertions(+), 6 deletions(-) delete mode 100644 src/hippopt/robot_planning/dynamics/__init__.py rename src/hippopt/robot_planning/{dynamics => expressions}/centroidal.py (100%) rename src/hippopt/robot_planning/{utilities => expressions}/quaternion.py (100%) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index c01fc8b8..bc2f4342 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -1,5 +1,5 @@ -from . import dynamics, expressions, utilities, variables -from .dynamics.centroidal import ( +from . import expressions, utilities, variables +from .expressions.centroidal import ( centroidal_dynamics_with_point_forces, com_dynamics_from_momentum, ) @@ -22,7 +22,7 @@ point_position_from_kinematics, quaternion_error_from_kinematics, ) -from .utilities.quaternion import ( +from .expressions.quaternion import ( quaternion_xyzw_error, quaternion_xyzw_normalization, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, diff --git a/src/hippopt/robot_planning/dynamics/__init__.py b/src/hippopt/robot_planning/dynamics/__init__.py deleted file mode 100644 index be83d941..00000000 --- a/src/hippopt/robot_planning/dynamics/__init__.py +++ /dev/null @@ -1 +0,0 @@ -from . import centroidal diff --git a/src/hippopt/robot_planning/dynamics/centroidal.py b/src/hippopt/robot_planning/expressions/centroidal.py similarity index 100% rename from src/hippopt/robot_planning/dynamics/centroidal.py rename to src/hippopt/robot_planning/expressions/centroidal.py diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index e3a84ef7..36fdd5ed 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -2,7 +2,7 @@ import liecasadi from adam.casadi import KinDynComputations -from hippopt.robot_planning.utilities.quaternion import ( +from hippopt.robot_planning.expressions.quaternion import ( quaternion_xyzw_error, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) diff --git a/src/hippopt/robot_planning/utilities/quaternion.py b/src/hippopt/robot_planning/expressions/quaternion.py similarity index 100% rename from src/hippopt/robot_planning/utilities/quaternion.py rename to src/hippopt/robot_planning/expressions/quaternion.py diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index f23da75e..3e725359 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -1 +1 @@ -from . import quaternion, terrain_descriptor +from . import terrain_descriptor From a5368e572001fdf6a0e8ac4da53e3b86b2a7f2cd Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 4 Oct 2023 18:13:18 +0200 Subject: [PATCH 040/106] Added possibility to set all contact points position from parent frame transform --- src/hippopt/robot_planning/variables/contacts.py | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 2ccd5be1..cd4e1aea 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -1,5 +1,6 @@ import dataclasses +import liecasadi import numpy as np from hippopt import ( @@ -89,6 +90,14 @@ def __post_init__(self) -> None: self.f_dot = np.zeros(3) +class FootContactState(list[ContactPointState]): + def set_from_parent_frame_transform(self, transform: liecasadi.SE3): + for contact_point in self: + contact_point.p = transform.translation() + transform.rotation().act( + contact_point.descriptor.position_in_foot_frame + ) + + @dataclasses.dataclass class FeetContactPointDescriptors: left: list[ContactPointDescriptor] = dataclasses.field(default_factory=list) @@ -97,5 +106,5 @@ class FeetContactPointDescriptors: @dataclasses.dataclass class FeetContactPoints(OptimizationObject): - left: list[ContactPointState] = default_composite_field(factory=list) - right: list[ContactPointState] = default_composite_field(factory=list) + left: FootContactState = default_composite_field(factory=FootContactState) + right: FootContactState = default_composite_field(factory=FootContactState) From c4640687848d1847b4dd5104785c23cf7824785c Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 10 Oct 2023 13:03:00 +0200 Subject: [PATCH 041/106] Added static method in FootContactState to set the points transform from the parent transform --- src/hippopt/robot_planning/__init__.py | 1 + src/hippopt/robot_planning/variables/contacts.py | 13 +++++++++++++ 2 files changed, 14 insertions(+) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index bc2f4342..e016975f 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -34,6 +34,7 @@ ContactPointStateDerivative, FeetContactPointDescriptors, FeetContactPoints, + FootContactState, ) from .variables.floating_base import ( FloatingBaseSystem, diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index cd4e1aea..b3197fd4 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -97,6 +97,19 @@ def set_from_parent_frame_transform(self, transform: liecasadi.SE3): contact_point.descriptor.position_in_foot_frame ) + @staticmethod + def from_parent_frame_transform( + descriptor: list[ContactPointDescriptor], transform: liecasadi.SE3 + ): + foot_contact_state = FootContactState() + for contact_point_descriptor in descriptor: + foot_contact_state.append( + ContactPointState(input_descriptor=contact_point_descriptor) + ) + + foot_contact_state.set_from_parent_frame_transform(transform) + return foot_contact_state + @dataclasses.dataclass class FeetContactPointDescriptors: From 50407af0ed4a9852e5ae373f8d755312e752d336 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 13 Oct 2023 18:35:58 +0200 Subject: [PATCH 042/106] Fixed setting of point_position_name default value --- src/hippopt/robot_planning/utilities/terrain_descriptor.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index 92594d07..6608ea15 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -9,9 +9,11 @@ class TerrainDescriptor(abc.ABC): _height_function: cs.Function = dataclasses.field(default=None) _normal_direction_function: cs.Function = dataclasses.field(default=None) _orientation_function: cs.Function = dataclasses.field(default=None) - _point_position_name: str = dataclasses.field(default=None) + _point_position_name: str = dataclasses.field(default="point_position") _options: dict = dataclasses.field(default=None) - point_position_name: dataclasses.InitVar[str] = dataclasses.field(default=None) + point_position_name: dataclasses.InitVar[str] = dataclasses.field( + default="point_position" + ) options: dataclasses.InitVar[dict] = dataclasses.field(default=None) def __post_init__( From 54d6744fc7a4d473950e15a1111190a154af0e99 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 13 Oct 2023 18:37:02 +0200 Subject: [PATCH 043/106] Added deps folder and imported surf2stl --- src/hippopt/deps/__init__.py | 1 + src/hippopt/deps/surf2stl/LICENSE.txt | 7 + src/hippopt/deps/surf2stl/__init__.py | 1 + src/hippopt/deps/surf2stl/surf2stl.py | 254 ++++++++++++++++++++++++++ 4 files changed, 263 insertions(+) create mode 100644 src/hippopt/deps/__init__.py create mode 100644 src/hippopt/deps/surf2stl/LICENSE.txt create mode 100644 src/hippopt/deps/surf2stl/__init__.py create mode 100644 src/hippopt/deps/surf2stl/surf2stl.py diff --git a/src/hippopt/deps/__init__.py b/src/hippopt/deps/__init__.py new file mode 100644 index 00000000..d3f05101 --- /dev/null +++ b/src/hippopt/deps/__init__.py @@ -0,0 +1 @@ +from .surf2stl import surf2stl diff --git a/src/hippopt/deps/surf2stl/LICENSE.txt b/src/hippopt/deps/surf2stl/LICENSE.txt new file mode 100644 index 00000000..f5a3f030 --- /dev/null +++ b/src/hippopt/deps/surf2stl/LICENSE.txt @@ -0,0 +1,7 @@ +Copyright 2020 asahidari + +Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. diff --git a/src/hippopt/deps/surf2stl/__init__.py b/src/hippopt/deps/surf2stl/__init__.py new file mode 100644 index 00000000..0b8a6306 --- /dev/null +++ b/src/hippopt/deps/surf2stl/__init__.py @@ -0,0 +1 @@ +from .surf2stl import write diff --git a/src/hippopt/deps/surf2stl/surf2stl.py b/src/hippopt/deps/surf2stl/surf2stl.py new file mode 100644 index 00000000..ec7fc542 --- /dev/null +++ b/src/hippopt/deps/surf2stl/surf2stl.py @@ -0,0 +1,254 @@ +### surf2stl.py --- Write a surface to a STL format file --- + +### Copyright (c) 2020 asahidari + +### This software is released under the MIT License. +### http://opensource.org/licenses/mit-license.php + +### Functions in this script (write, tri_write) export +### a stereolithography (STL) file for a surface with geometry +### defined by three matrix arguments, X, Y and Z. + +### This idea mainly come from surf2stl.m in MATLAB(or Octave): +### https://jp.mathworks.com/matlabcentral/fileexchange/4512-surf2stl + +import datetime +import math +import struct + +import numpy as np +from scipy.spatial import Delaunay + + +def write(filename, x, y, z, mode="binary"): + """ + Write a stl file for a surface with geometry + defined from three matrix arguments, x, y, and z. + Meshes are triangulated sequencially along xyz order. + + Parameters + ---------- + filename : string + output file name + + x, y, z : ndarray + Arguments x, y can be 1-dimensional arrays or 2-dimensional grids + (usually generated by np.meshgrid(x,y)), and z must be 2-dimensional, + which must have len(x)=n, len(y)=m where z.shape[m,n]. + + mode : string + STL file format, 'ascii' or 'binary'(default). + + Examples + ---------- + import numpy as np + import surf2stl + + x = np.linspace(-6, 6, 30) + y = np.linspace(-6, 6, 30) + X, Y = np.meshgrid(x, y) + Z = np.sin(np.sqrt(X ** 2 + Y ** 2)) + + surf2stl.write('3d-sinusoidal.stl', X, Y, Z) + + """ + + if type(filename) is not str: + raise Exception("Invalid filename") + + if mode != "ascii": + mode = "binary" + + if z.ndim != 2: + raise Exception("Variable z must be a 2-dimensional array") + + ### x, y can not be used as dx, dy in Python + ### if arguments type of x(or y) is 'int', + ### type error will raise in next 'if' block + # if type(x) == int and type(y) == int: + # x = np.arange(0, z.shape[1], x) + # x = np.arange(0, z.shape[0], y) + + if ( + len(x.shape) == 1 + and x.shape[0] == z.shape[1] + and len(y.shape) == 1 + and y.shape[0] == z.shape[0] + ): + x, y = np.meshgrid(x, y) + + if ( + len(x.shape) != len(z.shape) + or len(y.shape) != len(z.shape) + or x.shape[1] != z.shape[1] + or y.shape[0] != z.shape[0] + ): + raise Exception("Unable to resolve x and y variables") + + nfacets = 0 + title_str = "Created by surf2stl.py %s" % datetime.datetime.now().strftime( + "%d-%b-%Y %H:%M:%S" + ) + + f = open(filename, "wb" if mode != "ascii" else "w") + + if mode == "ascii": + f.write("solid %s\n" % title_str) + else: + title_str_ljust = title_str.ljust(80) + # f.write(title_str_ljust.encode('utf-8')) # same as 'ascii' for alphabet characters + f.write(title_str_ljust.encode("ascii")) + f.write(struct.pack("i", 0)) + + for i in range(z.shape[0] - 1): + for j in range(z.shape[1] - 1): + p1 = np.array([x[i, j], y[i, j], z[i, j]]) + p2 = np.array([x[i, j + 1], y[i, j + 1], z[i, j + 1]]) + p3 = np.array([x[i + 1, j + 1], y[i + 1, j + 1], z[i + 1, j + 1]]) + val = local_write_facet(f, p1, p2, p3, mode) + nfacets += val + + p1 = np.array([x[i + 1, j + 1], y[i + 1, j + 1], z[i + 1, j + 1]]) + p2 = np.array([x[i + 1, j], y[i + 1, j], z[i + 1, j]]) + p3 = np.array([x[i, j], y[i, j], z[i, j]]) + val = local_write_facet(f, p1, p2, p3, mode) + nfacets += val + + if mode == "ascii": + f.write("endsolid %s\n" % title_str) + else: + f.seek(80, 0) + f.write(struct.pack("i", nfacets)) + + f.close() + + print("Wrote %d facets" % nfacets) + return + + +def tri_write(filename, x, y, z, tri, mode="binary"): + """ + Write a stl file for a surface with geometry + defined from three matrix arguments, x, y, and z + with Delaunay Triangle parameter(tri). + + Meshes are triangulated with the 'tri' parameter + usually made with parameters which determine + the sequence of vertices (like [u, v]). + + Parameters + ---------- + filename : string + output file name + + x, y, z : ndarray + Each of these arguments must be 1-dimensional. + + tri : scipy.spatial.Delaunay + Delaunay Triangulation object. + When xyz coordinates are determined from other parameters(like (u, v)), + this triangle faces are basically calculated with the parameters. + + mode : string + STL file format, 'ascii' or 'binary'(default). + + Examples + ---------- + import numpy as np + from scipy.spatial import Delaunay + import surf2stl + + u = np.linspace(0, 2.0 * np.pi, endpoint=True, num=50) + v = np.linspace(-0.5, 0.5, endpoint=True, num=10) + u, v = np.meshgrid(u, v) + u, v = u.flatten(), v.flatten() + + x = (1 + 0.5 * v * np.cos(u / 2.0)) * np.cos(u) + y = (1 + 0.5 * v * np.cos(u / 2.0)) * np.sin(u) + z = 0.5 * v * np.sin(u / 2.0) + + delaunay_tri = Delaunay(np.array([u, v]).T) + surf2stl.tri_write('mobius.stl', x, y, z, delaunay_tri) + + """ + + if type(filename) is not str: + raise Exception("Invalid filename") + + if mode != "ascii": + mode = "binary" + + if len(x.shape) != 1 or len(y.shape) != 1 or len(z.shape) != 1: + raise Exception("Each variable x,y,z must be a 1-dimensional array") + + if x.shape[0] != z.shape[0] or y.shape[0] != z.shape[0]: + raise Exception("Number of x,y,z elements must be equal") + + nfacets = 0 + title_str = "Created by surf2stl.py %s" % datetime.datetime.now().strftime( + "%d-%b-%Y %H:%M:%S" + ) + + f = open(filename, "wb" if mode != "ascii" else "w") + + if mode == "ascii": + f.write("solid %s\n" % title_str) + else: + title_str_ljust = title_str.ljust(80) + # f.write(title_str_ljust.encode('utf-8')) # same as 'ascii' for alphabet characters + f.write(title_str_ljust.encode("ascii")) + f.write(struct.pack("i", 0)) + + indices = tri.simplices + verts = tri.points[indices] + for i in range(0, indices.shape[0], 1): + p = indices[i] + p1 = np.array([x[p[0]], y[p[0]], z[p[0]]]) + p2 = np.array([x[p[1]], y[p[1]], z[p[1]]]) + p3 = np.array([x[p[2]], y[p[2]], z[p[2]]]) + val = local_write_facet(f, p1, p2, p3, mode) + nfacets += val + + if mode == "ascii": + f.write("endsolid %s\n" % title_str) + else: + f.seek(80, 0) + f.write(struct.pack("i", nfacets)) + + f.close() + + print("Wrote %d facets" % nfacets) + return + + +# Local subfunctions + + +def local_write_facet(f, p1, p2, p3, mode): + if np.isnan(p1).any() or np.isnan(p2).any() or np.isnan(p3).any(): + return 0 + + n = local_find_normal(p1, p2, p3) + if mode == "ascii": + f.write("facet normal %.7f %.7f %.7f\n" % (n[0], n[1], n[2])) + f.write("outer loop\n") + f.write("vertex %.7f %.7f %.7f\n" % (p1[0], p1[1], p1[2])) + f.write("vertex %.7f %.7f %.7f\n" % (p2[0], p2[1], p2[2])) + f.write("vertex %.7f %.7f %.7f\n" % (p3[0], p3[1], p3[2])) + f.write("endloop\n") + f.write("endfacet\n") + else: + f.write(struct.pack("%sf" % len(n), *n)) + f.write(struct.pack("%sf" % len(p1), *p1)) + f.write(struct.pack("%sf" % len(p2), *p2)) + f.write(struct.pack("%sf" % len(p3), *p3)) + f.write(struct.pack("h", 0)) + return 1 + + +def local_find_normal(p1, p2, p3): + v1 = p2 - p1 + v2 = p3 - p1 + v3 = np.cross(v1, v2) + n = v3 / math.sqrt(np.sum(v3 * v3)) + return n From a7349698f315b0c791cde56afe90544d6d0fa139 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 13 Oct 2023 18:37:29 +0200 Subject: [PATCH 044/106] Added initial version of HumanStateVisualizer --- src/hippopt/robot_planning/__init__.py | 4 + .../robot_planning/utilities/__init__.py | 2 +- .../utilities/humanoid_state_visualizer.py | 184 ++++++++++++++++++ 3 files changed, 189 insertions(+), 1 deletion(-) create mode 100644 src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index e016975f..6de0176d 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -27,6 +27,10 @@ quaternion_xyzw_normalization, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) +from .utilities.humanoid_state_visualizer import ( + HumanoidStateVisualizer, + HumanoidStateVisualizerSettings, +) from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor from .variables.contacts import ( ContactPointDescriptor, diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index 3e725359..9e9d2764 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -1 +1 @@ -from . import terrain_descriptor +from . import humanoid_state_visualizer, terrain_descriptor diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py new file mode 100644 index 00000000..905c1ab1 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -0,0 +1,184 @@ +import dataclasses +import logging +import os + +import numpy as np +from idyntree.visualize import MeshcatVisualizer + +import hippopt.deps.surf2stl as surf2stl +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + + +@dataclasses.dataclass +class HumanoidStateVisualizerSettings: + robot_model: str = dataclasses.field(default=None) + considered_joints: list[str] = dataclasses.field(default=None) + robot_color: list[float] = dataclasses.field(default=None) + ground_color: list[float] = dataclasses.field(default=None) + terrain: TerrainDescriptor = dataclasses.field(default=None) + com_color: list[float] = dataclasses.field(default=None) + com_radius: float = dataclasses.field(default=None) + contact_points_color: list[float] = dataclasses.field(default=None) + contact_points_radius: float = dataclasses.field(default=None) + contact_forces_color: list[float] = dataclasses.field(default=None) + contact_force_radius: float = dataclasses.field(default=None) + working_folder: str = dataclasses.field(default=None) + ground_mesh_axis_points: int = dataclasses.field(default=None) + ground_x_limits: list[float] = dataclasses.field(default=None) + ground_y_limits: list[float] = dataclasses.field(default=None) + + def __post_init__(self): + self.robot_color = [1, 1, 1, 0.25] + self.ground_color = [0.5, 0.5, 0.5, 0.75] + self.com_color = [1, 0, 0, 1] + self.com_radius = 0.02 + self.contact_points_color = [0, 0, 0, 1] + self.contact_forces_color = [1, 0, 0, 1] + self.contact_points_radius = 0.01 + self.contact_force_radius = 0.005 + self.ground_x_limits = [-1.5, 1.5] + self.ground_y_limits = [-1.5, 1.5] + self.ground_mesh_axis_points = 200 + self.working_folder = "./" + + def is_valid(self): + ok = True + logger = logging.getLogger("[hippopt::HumanoidStateVisualizerSettings]") + if self.robot_model is None: + logger.error("robot_model is not specified.") + ok = False + if self.considered_joints is None: + logger.error("considered_joints is not specified.") + ok = False + if not os.access(self.working_folder, os.W_OK): + logger.error("working_folder is not writable.") + ok = False + if len(self.robot_color) != 4: + logger.error("robot_color is not specified correctly.") + ok = False + if len(self.ground_color) != 4: + logger.error("ground_color is not specified correctly.") + ok = False + if len(self.com_color) != 4: + logger.error("com_color is not specified correctly.") + ok = False + if len(self.contact_points_color) != 4: + logger.error("contact_points_color is not specified correctly.") + ok = False + if len(self.contact_forces_color) != 4: + logger.error("contact_forces_color is not specified correctly.") + ok = False + if self.com_radius <= 0: + logger.error("com_radius is not specified correctly.") + ok = False + if self.contact_points_radius <= 0: + logger.error("contact_points_radius is not specified correctly.") + ok = False + if self.contact_force_radius <= 0: + logger.error("contact_force_radius is not specified correctly.") + ok = False + if self.ground_mesh_axis_points <= 0: + logger.error("ground_mesh_axis_points is not specified correctly.") + ok = False + if self.terrain is None: + logger.error("terrain is not specified.") + ok = False + if len(self.ground_x_limits) != 2 or ( + self.ground_x_limits[0] >= self.ground_x_limits[1] + ): + logger.error("ground_x_limits are not specified correctly.") + ok = False + if len(self.ground_y_limits) != 2 or ( + self.ground_y_limits[0] >= self.ground_y_limits[1] + ): + logger.error("ground_y_limits are not specified correctly.") + ok = False + return ok + + +class HumanoidStateVisualizer: + def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: + if not settings.is_valid(): + raise ValueError("Settings are not valid.") + self._settings = settings + self.create_ground_urdf() + self.create_ground_mesh() + self._viz = MeshcatVisualizer() + self._viz.load_model_from_file( + model_path=settings.robot_model, + model_name="robot", + considered_joints=settings.considered_joints, + color=settings.robot_color, + ) + self._viz.load_model_from_file( + model_path=os.path.join(self._settings.working_folder, "ground.urdf"), + model_name="ground", + color=settings.ground_color, + ) + self._viz.load_sphere( + radius=settings.com_radius, shape_name="CoM", color=settings.com_color + ) + + def create_ground_urdf(self): + with open(os.path.join(self._settings.working_folder, "ground.urdf"), "w") as f: + f.write( + """ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + """ + ) + + def create_ground_mesh(self): + x_step = ( + self._settings.ground_x_limits[1] - self._settings.ground_x_limits[0] + ) / self._settings.ground_mesh_axis_points + y_step = ( + self._settings.ground_y_limits[1] - self._settings.ground_y_limits[0] + ) / self._settings.ground_mesh_axis_points + x = np.arange( + self._settings.ground_x_limits[0], + self._settings.ground_x_limits[1] + x_step, + x_step, + ) + y = np.arange( + self._settings.ground_y_limits[0], + self._settings.ground_y_limits[1] + y_step, + y_step, + ) + x, y = np.meshgrid(x, y) + assert x.shape == y.shape + points = np.array([x.flatten(), y.flatten(), np.zeros(x.size)]) + height_function_map = self._settings.terrain.height_function().map(x.size) + z = -np.array(height_function_map(points).full()).reshape(x.shape) + surf2stl.write( + os.path.join(self._settings.working_folder, "ground.stl"), x, y, z + ) From 2d16e66bc648c67479d2d8568c67fb3f24e14fbf Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 14 Oct 2023 11:52:46 +0200 Subject: [PATCH 045/106] Added visualization of the HumanoidState --- .../utilities/humanoid_state_visualizer.py | 70 +++++++++++++++++++ 1 file changed, 70 insertions(+) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 905c1ab1..81f624d8 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -2,17 +2,24 @@ import logging import os +import idyntree.bindings +import liecasadi import numpy as np from idyntree.visualize import MeshcatVisualizer import hippopt.deps.surf2stl as surf2stl from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor +from hippopt.robot_planning.variables.humanoid import ( + FeetContactPointDescriptors, + HumanoidState, +) @dataclasses.dataclass class HumanoidStateVisualizerSettings: robot_model: str = dataclasses.field(default=None) considered_joints: list[str] = dataclasses.field(default=None) + contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) robot_color: list[float] = dataclasses.field(default=None) ground_color: list[float] = dataclasses.field(default=None) terrain: TerrainDescriptor = dataclasses.field(default=None) @@ -22,6 +29,7 @@ class HumanoidStateVisualizerSettings: contact_points_radius: float = dataclasses.field(default=None) contact_forces_color: list[float] = dataclasses.field(default=None) contact_force_radius: float = dataclasses.field(default=None) + contact_force_scaling: float = dataclasses.field(default=None) working_folder: str = dataclasses.field(default=None) ground_mesh_axis_points: int = dataclasses.field(default=None) ground_x_limits: list[float] = dataclasses.field(default=None) @@ -36,6 +44,7 @@ def __post_init__(self): self.contact_forces_color = [1, 0, 0, 1] self.contact_points_radius = 0.01 self.contact_force_radius = 0.005 + self.contact_force_scaling = 0.01 self.ground_x_limits = [-1.5, 1.5] self.ground_y_limits = [-1.5, 1.5] self.ground_mesh_axis_points = 200 @@ -50,6 +59,9 @@ def is_valid(self): if self.considered_joints is None: logger.error("considered_joints is not specified.") ok = False + if self.contact_points is None: + logger.error("contact_points is not specified.") + ok = False if not os.access(self.working_folder, os.W_OK): logger.error("working_folder is not writable.") ok = False @@ -118,6 +130,20 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: self._viz.load_sphere( radius=settings.com_radius, shape_name="CoM", color=settings.com_color ) + for i, point in enumerate( + (settings.contact_points.left + settings.contact_points.right) + ): + self._viz.load_sphere( + radius=settings.contact_points_radius, + shape_name=f"p_{i}", + color=settings.contact_points_color, + ) + self._viz.load_cylinder( + radius=settings.contact_force_radius, + length=1.0, + shape_name=f"f_{i}", + color=settings.contact_forces_color, + ) def create_ground_urdf(self): with open(os.path.join(self._settings.working_folder, "ground.urdf"), "w") as f: @@ -182,3 +208,47 @@ def create_ground_mesh(self): surf2stl.write( os.path.join(self._settings.working_folder, "ground.stl"), x, y, z ) + + def visualize(self, state: HumanoidState): + self._viz.set_multibody_system_state( + state.kinematics.base.position, + liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) + .as_matrix() + .full(), + state.kinematics.joints.positions, + "robot", + ) + self._viz.set_primitive_geometry_transform( + state.com, + np.eye(3), + "CoM", + ) + for i, point in enumerate( + (state.contact_points.left + state.contact_points.right) + ): + self._viz.set_primitive_geometry_transform( + point.p, + np.eye(3), + f"p_{i}", + ) + force_norm = np.linalg.norm(point.f) + force_direction = ( + point.f / force_norm if force_norm > 0 else np.array([0, 0, 1]) + ) + direction = idyntree.bindings.Direction() + direction.FromPython(force_direction) + angle = np.arccos(np.dot(np.array([0, 0, 1]), force_direction)) + rotation = idyntree.bindings.Rotation.RotAxis(direction, angle).toNumPy() + scaling = np.diag([1, 1, self._settings.contact_force_scaling * force_norm]) + position = ( + point.p + scaling @ force_direction / 2 + ) # the origin is in the cylinder center + transform = ( + liecasadi.SE3.from_translation_and_rotation( + position, liecasadi.SO3.from_matrix(rotation) + ) + .as_matrix() + .full() + ) + transform[0:3, 0:3] = rotation @ scaling + self._viz.viewer[f"f_{i}"].set_transform(transform) From 0ab8bc56a6be58a4c04cc19f3196d7b69faf7e73 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sun, 15 Oct 2023 13:30:31 +0200 Subject: [PATCH 046/106] Avoiding to convert a rotation matrix to quaternion --- src/hippopt/robot_planning/__init__.py | 2 +- .../robot_planning/expressions/kinematics.py | 14 +++++++------- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 6de0176d..ccc339c0 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -20,7 +20,7 @@ centroidal_momentum_from_kinematics, frames_relative_position, point_position_from_kinematics, - quaternion_error_from_kinematics, + rotation_error_from_kinematics, ) from .expressions.quaternion import ( quaternion_xyzw_error, diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index 36fdd5ed..db950fd0 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -202,7 +202,7 @@ def frames_relative_position( ) -def quaternion_error_from_kinematics( +def rotation_error_from_kinematics( kindyn_object: KinDynComputations, target_frame: str, base_position_name: str = "base_position", @@ -224,11 +224,11 @@ def quaternion_error_from_kinematics( target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) target_pose = target_fk_function(base_pose, joint_positions) + target_orientation = target_pose[:3, :3] - target_quaternion = liecasadi.SO3.from_matrix(target_pose[:3, :3]).as_quat() - quaternion_error_fun = quaternion_xyzw_error(options=options) - - error = quaternion_error_fun(target_quaternion, desired_quaternion) + rotation_error = ( + target_orientation @ liecasadi.SO3.from_quat(desired_quaternion).as_matrix().T + ) return cs.Function( "quaternion_error_from_kinematics", @@ -238,13 +238,13 @@ def quaternion_error_from_kinematics( joint_positions, desired_quaternion, ], - [error], + [rotation_error], [ base_position_name, base_quaternion_xyzw_name, joint_positions_name, desired_quaternion_xyzw_name, ], - ["quaternion_error"], + ["rotation_error"], options, ) From 93841675249e78d07836ee66b59cbd611d058d0d Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 15:45:42 +0200 Subject: [PATCH 047/106] Removed unused import --- src/hippopt/robot_planning/expressions/kinematics.py | 1 - 1 file changed, 1 deletion(-) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index db950fd0..f3bec036 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -3,7 +3,6 @@ from adam.casadi import KinDynComputations from hippopt.robot_planning.expressions.quaternion import ( - quaternion_xyzw_error, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) From 6b08bfa4498a9560575355f7dd3bb6d3f5ebcdfe Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 16 Oct 2023 18:08:14 +0200 Subject: [PATCH 048/106] Avoid using from_matrix when computing frame relative position --- src/hippopt/robot_planning/expressions/kinematics.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/kinematics.py b/src/hippopt/robot_planning/expressions/kinematics.py index f3bec036..bae777a4 100644 --- a/src/hippopt/robot_planning/expressions/kinematics.py +++ b/src/hippopt/robot_planning/expressions/kinematics.py @@ -180,15 +180,17 @@ def frames_relative_position( reference_fk_function = kindyn_object.forward_kinematics_fun(frame=reference_frame) target_fk_function = kindyn_object.forward_kinematics_fun(frame=target_frame) - reference_pose = liecasadi.SE3.from_matrix( - reference_fk_function(base_pose, joint_positions) + reference_pose = reference_fk_function(base_pose, joint_positions) + reference_pose_inverse_rotation = reference_pose[:3, :3].T + reference_pose_inverse_translation = ( + -reference_pose_inverse_rotation @ reference_pose[:3, 3] ) - reference_pose_inverse = reference_pose.inverse() + target_pose = target_fk_function(base_pose, joint_positions) relative_position = ( - reference_pose_inverse.rotation().act(target_pose[:3, 3]) - + reference_pose_inverse.translation() + reference_pose_inverse_rotation @ target_pose[:3, 3] + + reference_pose_inverse_translation ) return cs.Function( From 3bd8d80b734b650bfcd481c218bffbe694a5cb13 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 10:12:58 +0200 Subject: [PATCH 049/106] Added draft to visualize multiple steps in a row --- .../utilities/humanoid_state_visualizer.py | 39 ++++++++++++++++--- 1 file changed, 34 insertions(+), 5 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 81f624d8..5ce9d953 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -1,6 +1,7 @@ import dataclasses import logging import os +import time import idyntree.bindings import liecasadi @@ -113,8 +114,8 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: if not settings.is_valid(): raise ValueError("Settings are not valid.") self._settings = settings - self.create_ground_urdf() - self.create_ground_mesh() + self._create_ground_urdf() + self._create_ground_mesh() self._viz = MeshcatVisualizer() self._viz.load_model_from_file( model_path=settings.robot_model, @@ -145,7 +146,7 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: color=settings.contact_forces_color, ) - def create_ground_urdf(self): + def _create_ground_urdf(self): with open(os.path.join(self._settings.working_folder, "ground.urdf"), "w") as f: f.write( """ @@ -183,7 +184,7 @@ def create_ground_urdf(self): """ ) - def create_ground_mesh(self): + def _create_ground_mesh(self): x_step = ( self._settings.ground_x_limits[1] - self._settings.ground_x_limits[0] ) / self._settings.ground_mesh_axis_points @@ -209,7 +210,7 @@ def create_ground_mesh(self): os.path.join(self._settings.working_folder, "ground.stl"), x, y, z ) - def visualize(self, state: HumanoidState): + def _visualize_single_state(self, state: HumanoidState): self._viz.set_multibody_system_state( state.kinematics.base.position, liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) @@ -252,3 +253,31 @@ def visualize(self, state: HumanoidState): ) transform[0:3, 0:3] = rotation @ scaling self._viz.viewer[f"f_{i}"].set_transform(transform) + + def _visualize_multiple_states( + self, states: list[HumanoidState], timestep_ms: float | list[float] = None + ): + if timestep_ms is None or isinstance(timestep_ms, float): + single_step = timestep_ms if timestep_ms is not None else 0.0 + timestep_ms = [single_step] * len(states) + + if len(timestep_ms) != len(states): + raise ValueError("timestep and states have different lengths.") + + for i, state in enumerate(states): + start = time.time() + self._visualize_single_state(state) + end = time.time() + elapsed_s = end - start + sleep_time = timestep_ms[i] * 1000 - elapsed_s + time.sleep(max(0.0, sleep_time)) + + def visualize( + self, + state: HumanoidState | list[HumanoidState], + timestep_ms: float | list[float] = None, + ): + if isinstance(state, list): + self._visualize_multiple_states(state, timestep_ms=timestep_ms) + else: + self._visualize_single_state(state) From 4f4ecf8f1db69a617e642d1d883119b9e9afe9a6 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 13:02:12 +0200 Subject: [PATCH 050/106] Added possibility to set time_multiplier in the states visualization --- .../utilities/humanoid_state_visualizer.py | 22 ++++++++++++------- 1 file changed, 14 insertions(+), 8 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 5ce9d953..6c708cf0 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -255,13 +255,16 @@ def _visualize_single_state(self, state: HumanoidState): self._viz.viewer[f"f_{i}"].set_transform(transform) def _visualize_multiple_states( - self, states: list[HumanoidState], timestep_ms: float | list[float] = None + self, + states: list[HumanoidState], + timestep_s: float | list[float] | np.ndarray = None, + time_multiplier: float = 1.0, ): - if timestep_ms is None or isinstance(timestep_ms, float): - single_step = timestep_ms if timestep_ms is not None else 0.0 - timestep_ms = [single_step] * len(states) + if timestep_s is None or isinstance(timestep_s, float) or timestep_s.size == 1: + single_step = timestep_s if timestep_s is not None else 0.0 + timestep_s = [single_step] * len(states) - if len(timestep_ms) != len(states): + if len(timestep_s) != len(states): raise ValueError("timestep and states have different lengths.") for i, state in enumerate(states): @@ -269,15 +272,18 @@ def _visualize_multiple_states( self._visualize_single_state(state) end = time.time() elapsed_s = end - start - sleep_time = timestep_ms[i] * 1000 - elapsed_s + sleep_time = timestep_s[i] * time_multiplier - elapsed_s time.sleep(max(0.0, sleep_time)) def visualize( self, state: HumanoidState | list[HumanoidState], - timestep_ms: float | list[float] = None, + timestep_s: float | list[float] | np.ndarray = None, + time_multiplier: float = 1.0, ): if isinstance(state, list): - self._visualize_multiple_states(state, timestep_ms=timestep_ms) + self._visualize_multiple_states( + state, timestep_s=timestep_s, time_multiplier=time_multiplier + ) else: self._visualize_single_state(state) From 2c9b51c2f0c54318b61fa77d93842123ca2e25b1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 17:57:57 +0200 Subject: [PATCH 051/106] Changed visualization of arrows --- .../utilities/humanoid_state_visualizer.py | 49 +++++++++++++------ 1 file changed, 33 insertions(+), 16 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 6c708cf0..b5ed8a5c 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -3,7 +3,6 @@ import os import time -import idyntree.bindings import liecasadi import numpy as np from idyntree.visualize import MeshcatVisualizer @@ -113,6 +112,7 @@ class HumanoidStateVisualizer: def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: if not settings.is_valid(): raise ValueError("Settings are not valid.") + self._logger = logging.getLogger("[hippopt::HumanoidStateVisualizer]") self._settings = settings self._create_ground_urdf() self._create_ground_mesh() @@ -210,6 +210,16 @@ def _create_ground_mesh(self): os.path.join(self._settings.working_folder, "ground.stl"), x, y, z ) + @staticmethod + def _skew(x: np.ndarray) -> np.ndarray: + return np.array( + [ + [0, -x[2], x[1]], + [x[2], 0, -x[0]], + [-x[1], x[0], 0], + ] + ) + def _visualize_single_state(self, state: HumanoidState): self._viz.set_multibody_system_state( state.kinematics.base.position, @@ -224,6 +234,7 @@ def _visualize_single_state(self, state: HumanoidState): np.eye(3), "CoM", ) + for i, point in enumerate( (state.contact_points.left + state.contact_points.right) ): @@ -232,25 +243,30 @@ def _visualize_single_state(self, state: HumanoidState): np.eye(3), f"p_{i}", ) + force_norm = np.linalg.norm(point.f) - force_direction = ( - point.f / force_norm if force_norm > 0 else np.array([0, 0, 1]) - ) - direction = idyntree.bindings.Direction() - direction.FromPython(force_direction) - angle = np.arccos(np.dot(np.array([0, 0, 1]), force_direction)) - rotation = idyntree.bindings.Rotation.RotAxis(direction, angle).toNumPy() + + if force_norm < 1e-6: + rotation = np.eye(3) + else: + force_direction = point.f / force_norm + cos_angle = np.dot(np.array([0, 0, 1]), force_direction) + rotation_axis = self._skew(np.array([0, 0, 1])) @ force_direction + skew_symmetric_matrix = self._skew(rotation_axis) + rotation = ( + np.eye(3) + + skew_symmetric_matrix + + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) + * ((1 - cos_angle) / (force_norm**2)) + ) + scaling = np.diag([1, 1, self._settings.contact_force_scaling * force_norm]) position = ( - point.p + scaling @ force_direction / 2 + point.p + point.f * self._settings.contact_force_scaling / 2 ) # the origin is in the cylinder center - transform = ( - liecasadi.SE3.from_translation_and_rotation( - position, liecasadi.SO3.from_matrix(rotation) - ) - .as_matrix() - .full() - ) + transform = np.zeros((4, 4)) + transform[0:3, 3] = position + transform[3, 3] = 1 transform[0:3, 0:3] = rotation @ scaling self._viz.viewer[f"f_{i}"].set_transform(transform) @@ -268,6 +284,7 @@ def _visualize_multiple_states( raise ValueError("timestep and states have different lengths.") for i, state in enumerate(states): + self._logger.info(f"Visualizing state {i + 1}/{len(states)}") start = time.time() self._visualize_single_state(state) end = time.time() From 1ebf3f98b6e4c9711c89e36bd0be4c80356929b9 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 17 Oct 2023 18:52:16 +0200 Subject: [PATCH 052/106] Added possibility to use the mass regularized version of the centroidal dynamics --- src/hippopt/robot_planning/expressions/centroidal.py | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/centroidal.py b/src/hippopt/robot_planning/expressions/centroidal.py index f9000d6d..010eff85 100644 --- a/src/hippopt/robot_planning/expressions/centroidal.py +++ b/src/hippopt/robot_planning/expressions/centroidal.py @@ -3,6 +3,7 @@ def centroidal_dynamics_with_point_forces( number_of_points: int, + assume_unitary_mass: bool = False, mass_name: str = "m", gravity_name: str = "g", com_name: str = "com", @@ -29,8 +30,11 @@ def centroidal_dynamics_with_point_forces( input_vars = [] - m = cs.MX.sym(mass_name, 1) - input_vars.append(m) + if assume_unitary_mass: + m = 1.0 + else: + m = cs.MX.sym(mass_name, 1) + input_vars.append(m) g = cs.MX.sym(gravity_name, 6) input_vars.append(g) From dc01729d54f7764bc1bf22b783512c066487ef1e Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 18 Oct 2023 15:30:52 +0200 Subject: [PATCH 053/106] Added possibility to save frames and video in the visualizer --- .../utilities/humanoid_state_visualizer.py | 96 ++++++++++++++++--- 1 file changed, 84 insertions(+), 12 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index b5ed8a5c..811132a8 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -1,8 +1,11 @@ +import copy import dataclasses import logging import os +import pathlib import time +import ffmpeg import liecasadi import numpy as np from idyntree.visualize import MeshcatVisualizer @@ -220,7 +223,9 @@ def _skew(x: np.ndarray) -> np.ndarray: ] ) - def _visualize_single_state(self, state: HumanoidState): + def _visualize_single_state( + self, state: HumanoidState, save: bool, file_name_stem: str + ): self._viz.set_multibody_system_state( state.kinematics.base.position, liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) @@ -244,6 +249,9 @@ def _visualize_single_state(self, state: HumanoidState): f"p_{i}", ) + # Copied from https://github.com/robotology/idyntree/pull/1087 until it is + # available in conda + force_norm = np.linalg.norm(point.f) if force_norm < 1e-6: @@ -270,37 +278,101 @@ def _visualize_single_state(self, state: HumanoidState): transform[0:3, 0:3] = rotation @ scaling self._viz.viewer[f"f_{i}"].set_transform(transform) + if save: + image = self._viz.viewer.get_image() + image.save(file_name_stem + ".png") + def _visualize_multiple_states( self, states: list[HumanoidState], - timestep_s: float | list[float] | np.ndarray = None, - time_multiplier: float = 1.0, + timestep_s: float | list[float] | np.ndarray, + time_multiplier: float, + save: bool, + file_name_stem: str, ): - if timestep_s is None or isinstance(timestep_s, float) or timestep_s.size == 1: - single_step = timestep_s if timestep_s is not None else 0.0 - timestep_s = [single_step] * len(states) + _timestep_s = copy.deepcopy(timestep_s) + if ( + _timestep_s is None + or isinstance(_timestep_s, float) + or _timestep_s.size == 1 + ): + single_step = _timestep_s if _timestep_s is not None else 0.0 + _timestep_s = [single_step] * len(states) + + if len(_timestep_s) != len(states): + raise ValueError("timestep_s and states have different lengths.") - if len(timestep_s) != len(states): - raise ValueError("timestep and states have different lengths.") + folder_name = f"{self._settings.working_folder}/{file_name_stem}_frames" + pathlib.Path(folder_name).mkdir(parents=True, exist_ok=True) for i, state in enumerate(states): self._logger.info(f"Visualizing state {i + 1}/{len(states)}") start = time.time() - self._visualize_single_state(state) + self._visualize_single_state( + state, + save=save, + file_name_stem=f"{folder_name}/frame_{i:03}", + ) end = time.time() elapsed_s = end - start - sleep_time = timestep_s[i] * time_multiplier - elapsed_s + sleep_time = _timestep_s[i] * time_multiplier - elapsed_s time.sleep(max(0.0, sleep_time)) + if save: + if timestep_s is None: + self._logger.warning("timestep_s is None. Saving video with 1.0 fps.") + fps = 1.0 + elif isinstance(timestep_s, list): + if len(timestep_s) > 1: + self._logger.warning( + "The input timestep is a list. " + "Using the average to compute the fps." + ) + fps = 1.0 / (sum(timestep_s) / len(timestep_s)) + else: + fps = 1.0 / timestep_s[0] + elif isinstance(timestep_s, np.ndarray): + if timestep_s.size > 1: + self._logger.warning( + "The input timestep is a list. " + "Using the average to compute the fps." + ) + fps = 1.0 / (sum(timestep_s) / len(timestep_s)) + else: + fps = 1.0 / timestep_s + elif isinstance(timestep_s, float): + fps = 1.0 / timestep_s + else: + self._logger.warning("Using the fps=1.0") + fps = 1.0 + + frames = ffmpeg.input( + f"{folder_name}/frame_*.png", pattern_type="glob", framerate=fps + ) + video = ffmpeg.output( + frames, + f"{self._settings.working_folder}/{file_name_stem}.mp4", + video_bitrate="20M", + ) + ffmpeg.run(video) + def visualize( self, state: HumanoidState | list[HumanoidState], timestep_s: float | list[float] | np.ndarray = None, time_multiplier: float = 1.0, + save: bool = False, + file_name_stem: str = "humanoid_state_visualization", ): if isinstance(state, list): self._visualize_multiple_states( - state, timestep_s=timestep_s, time_multiplier=time_multiplier + state, + timestep_s=timestep_s, + time_multiplier=time_multiplier, + save=save, + file_name_stem=file_name_stem, ) else: - self._visualize_single_state(state) + self._visualize_single_state( + state, save=save, file_name_stem=file_name_stem + ) From 5b8614db665fd07f576ea3907e6010c259309518 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 30 Oct 2023 19:09:14 +0100 Subject: [PATCH 054/106] Added first version of plotter for complementarity --- .../utilities/foot_contact_state_plotter.py | 157 ++++++++++++++++++ 1 file changed, 157 insertions(+) create mode 100644 src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py new file mode 100644 index 00000000..d067c1c0 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -0,0 +1,157 @@ +import copy +import dataclasses +import math + +import matplotlib.axes +import matplotlib.pyplot as plt +import numpy as np + +from hippopt.robot_planning.utilities.terrain_descriptor import ( + PlanarTerrain, + TerrainDescriptor, +) +from hippopt.robot_planning.variables.contacts import ( + ContactPointState, + FootContactState, +) + + +@dataclasses.dataclass +class ContactPointStatePlotterSettings: + axes: matplotlib.axes.Axes = dataclasses.field(default=None) + terrain: TerrainDescriptor = dataclasses.field(default=None) + + input_axes: dataclasses.InitVar[matplotlib.axes.Axes] = dataclasses.field( + default=None + ) + input_terrain: dataclasses.InitVar[TerrainDescriptor] = dataclasses.field( + default=None + ) + + def __post_init__( + self, input_axes: matplotlib.axes.Axes, input_terrain: TerrainDescriptor + ): + self.axes = input_axes + self.terrain = ( + input_terrain + if isinstance(input_terrain, TerrainDescriptor) + else PlanarTerrain() + ) + + +class ContactPointStatePlotter: + def __init__( + self, + settings: ContactPointStatePlotterSettings = ContactPointStatePlotterSettings(), + ): + self.settings = settings + self._axes = self.settings.axes + self._fig = None + + def plot_complementarity( + self, + states: list[ContactPointState], + time_s: float | list[float] | np.ndarray = None, + title: str = "Contact Point Complementarity", + ): + _time_s = copy.deepcopy(time_s) + if _time_s is None or isinstance(_time_s, float) or _time_s.size == 1: + single_step = _time_s if _time_s is not None else 0.0 + _time_s = np.linspace(0, len(states) * single_step, len(states)) + + if len(_time_s) != len(states): + raise ValueError( + "timestep_s and foot_contact_states have different lengths." + ) + + if self._axes is None: + self._fig, self._axes = plt.subplots() + + height_function = self.settings.terrain.height_function() + normal_direction_fun = self.settings.terrain.normal_direction_function() + + positions = [height_function(s.p) for s in states] + forces = [normal_direction_fun(s.p).T @ s.f for s in states] + self._axes.plot(_time_s, positions) + self._axes.set_ylabel("Height [m]", color="C0") + self._axes.tick_params(axis="y", color="C0", labelcolor="C0") + axes_force = self._axes.twinx() + axes_force.plot(_time_s, forces, "C1") + axes_force.set_ylabel("Normal Force [N]", color="C1") + axes_force.tick_params(axis="y", color="C1", labelcolor="C1") + axes_force.spines["right"].set_color("C1") + axes_force.spines["left"].set_color("C0") + + if self._fig is not None: + self._fig.suptitle(title) + plt.draw() + plt.pause(0.001) + + +@dataclasses.dataclass +class FootContactStatePlotterSettings: + number_of_columns: int = dataclasses.field(default=-1) + terrain: TerrainDescriptor = dataclasses.field(default=None) + + +class FootContactStatePlotter: + def __init__( + self, + settings: FootContactStatePlotterSettings = FootContactStatePlotterSettings(), + ): + self.settings = settings + self.number_of_rows = -1 + self.fig = None + self.point_plotters = [] + + def plot_complementarity( + self, + states: list[FootContactState], + time_s: float | list[float] | np.ndarray = None, + title: str = "Foot Contact Complementarity", + ): + _time_s = copy.deepcopy(time_s) + if _time_s is None or isinstance(_time_s, float) or _time_s.size == 1: + single_step = _time_s if _time_s is not None else 0.0 + _time_s = np.linspace(0, len(states) * single_step, len(states)) + + if len(_time_s) != len(states): + raise ValueError( + "timestep_s and foot_contact_states have different lengths." + ) + + if len(states) == 0: + return + + number_of_plots = len(states[0]) + if self.settings.number_of_columns < 1: + self.settings.number_of_columns = math.ceil(math.sqrt(number_of_plots)) + number_of_rows = math.ceil(number_of_plots / self.settings.number_of_columns) + + if self.number_of_rows != number_of_rows: + self.fig, axes_list = plt.subplots( + nrows=number_of_rows, + ncols=self.settings.number_of_columns, + squeeze=False, + ) + self.number_of_rows = number_of_rows + self.point_plotters = [ + ContactPointStatePlotter( + ContactPointStatePlotterSettings( + input_axes=el, terrain=self.settings.terrain + ) + ) + for row in axes_list + for el in row + ] + assert len(self.point_plotters) == number_of_plots + + for p in range(number_of_plots): + contact_states = [state[p] for state in states] + self.point_plotters[p].plot_complementarity( + states=contact_states, time_s=_time_s + ) + + self.fig.suptitle(title) + plt.draw() + plt.pause(0.001) From 13975d5a172dbdad5f230b6705927bb520975fb1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 31 Oct 2023 13:00:41 +0100 Subject: [PATCH 055/106] Added visualization of complementarity. Exploiting external processes to avoid matplotlib blocking the execution --- src/hippopt/robot_planning/__init__.py | 6 + .../robot_planning/utilities/__init__.py | 2 +- .../utilities/foot_contact_state_plotter.py | 103 ++++++++++++------ .../utilities/humanoid_state_visualizer.py | 7 ++ 4 files changed, 86 insertions(+), 32 deletions(-) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index ccc339c0..b3557ec9 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -27,6 +27,12 @@ quaternion_xyzw_normalization, quaternion_xyzw_velocity_to_right_trivialized_angular_velocity, ) +from .utilities.foot_contact_state_plotter import ( + ContactPointStatePlotter, + ContactPointStatePlotterSettings, + FootContactStatePlotter, + FootContactStatePlotterSettings, +) from .utilities.humanoid_state_visualizer import ( HumanoidStateVisualizer, HumanoidStateVisualizerSettings, diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index 9e9d2764..a46e1497 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -1 +1 @@ -from . import humanoid_state_visualizer, terrain_descriptor +from . import foot_contact_state_plotter, humanoid_state_visualizer, terrain_descriptor diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py index d067c1c0..2d7aff0b 100644 --- a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -1,6 +1,9 @@ import copy import dataclasses +import logging import math +import multiprocessing +from typing import TypeVar import matplotlib.axes import matplotlib.pyplot as plt @@ -70,8 +73,8 @@ def plot_complementarity( height_function = self.settings.terrain.height_function() normal_direction_fun = self.settings.terrain.normal_direction_function() - positions = [height_function(s.p) for s in states] - forces = [normal_direction_fun(s.p).T @ s.f for s in states] + positions = np.array([height_function(s.p) for s in states]).flatten() + forces = np.array([normal_direction_fun(s.p).T @ s.f for s in states]).flatten() self._axes.plot(_time_s, positions) self._axes.set_ylabel("Height [m]", color="C0") self._axes.tick_params(axis="y", color="C0", labelcolor="C0") @@ -86,6 +89,7 @@ def plot_complementarity( self._fig.suptitle(title) plt.draw() plt.pause(0.001) + plt.show() @dataclasses.dataclass @@ -94,64 +98,101 @@ class FootContactStatePlotterSettings: terrain: TerrainDescriptor = dataclasses.field(default=None) +TFootContactStatePlotter = TypeVar( + "TFootContactStatePlotter", bound="FootContactStatePlotter" +) + + class FootContactStatePlotter: def __init__( self, settings: FootContactStatePlotterSettings = FootContactStatePlotterSettings(), ): - self.settings = settings - self.number_of_rows = -1 - self.fig = None - self.point_plotters = [] + self._settings = settings + self._ext_process = None + self._logger = logging.getLogger("[hippopt::FootContactStatePlotter]") def plot_complementarity( self, states: list[FootContactState], time_s: float | list[float] | np.ndarray = None, title: str = "Foot Contact Complementarity", + blocking: bool = False, ): + if self._ext_process is not None: + self._logger.warning( + "A plot is already running. " + "Make sure to close the previous plot first." + ) + self._ext_process.join() + self._ext_process = None _time_s = copy.deepcopy(time_s) + _states = copy.deepcopy(states) + _terrain = copy.deepcopy(self._settings.terrain) if _time_s is None or isinstance(_time_s, float) or _time_s.size == 1: single_step = _time_s if _time_s is not None else 0.0 _time_s = np.linspace(0, len(states) * single_step, len(states)) - if len(_time_s) != len(states): + if len(_time_s) != len(_states): raise ValueError( "timestep_s and foot_contact_states have different lengths." ) - if len(states) == 0: + if len(_states) == 0: return + self._ext_process = multiprocessing.Process( + target=FootContactStatePlotter._create_complementarity_plot, + args=( + _states, + _time_s, + title, + self._settings.number_of_columns, + _terrain, + ), + ) + self._ext_process.start() + + if blocking: + self._ext_process.join() + + @staticmethod + def _create_complementarity_plot( + states: list[FootContactState], + time_s: np.ndarray, + title: str, + number_of_columns: int, + terrain: TerrainDescriptor, + ): number_of_plots = len(states[0]) - if self.settings.number_of_columns < 1: - self.settings.number_of_columns = math.ceil(math.sqrt(number_of_plots)) - number_of_rows = math.ceil(number_of_plots / self.settings.number_of_columns) - - if self.number_of_rows != number_of_rows: - self.fig, axes_list = plt.subplots( - nrows=number_of_rows, - ncols=self.settings.number_of_columns, - squeeze=False, + _number_of_columns = ( + math.ceil(math.sqrt(number_of_plots)) + if number_of_columns < 1 + else number_of_columns + ) + number_of_rows = math.ceil(number_of_plots / _number_of_columns) + + _fig, axes_list = plt.subplots( + nrows=number_of_rows, + ncols=_number_of_columns, + squeeze=False, + ) + _point_plotters = [ + ContactPointStatePlotter( + ContactPointStatePlotterSettings(input_axes=el, terrain=terrain) ) - self.number_of_rows = number_of_rows - self.point_plotters = [ - ContactPointStatePlotter( - ContactPointStatePlotterSettings( - input_axes=el, terrain=self.settings.terrain - ) - ) - for row in axes_list - for el in row - ] - assert len(self.point_plotters) == number_of_plots + for row in axes_list + for el in row + ] + assert len(_point_plotters) == number_of_plots for p in range(number_of_plots): contact_states = [state[p] for state in states] - self.point_plotters[p].plot_complementarity( - states=contact_states, time_s=_time_s + _point_plotters[p].plot_complementarity( + states=contact_states, time_s=time_s ) - self.fig.suptitle(title) + _fig.suptitle(title) plt.draw() plt.pause(0.001) + plt.show() diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 811132a8..d6625aa5 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -305,6 +305,13 @@ def _visualize_multiple_states( folder_name = f"{self._settings.working_folder}/{file_name_stem}_frames" pathlib.Path(folder_name).mkdir(parents=True, exist_ok=True) + if save: + self._logger.info( + f"Saving visualization frames in {folder_name}. " + "Make sure to have the visualizer open, " + "otherwise the process will hang." + ) + for i, state in enumerate(states): self._logger.info(f"Visualizing state {i + 1}/{len(states)}") start = time.time() From 8ec46841f19cc917bc80e69565b71d45d93f1e13 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 31 Oct 2023 15:20:27 +0100 Subject: [PATCH 056/106] Added visualization of complementarity error --- .../utilities/foot_contact_state_plotter.py | 47 +++++++++++++------ 1 file changed, 33 insertions(+), 14 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py index 2d7aff0b..919c3ad1 100644 --- a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -21,10 +21,10 @@ @dataclasses.dataclass class ContactPointStatePlotterSettings: - axes: matplotlib.axes.Axes = dataclasses.field(default=None) + axes: list[matplotlib.axes.Axes] | None = dataclasses.field(default=None) terrain: TerrainDescriptor = dataclasses.field(default=None) - input_axes: dataclasses.InitVar[matplotlib.axes.Axes] = dataclasses.field( + input_axes: dataclasses.InitVar[list[matplotlib.axes.Axes]] = dataclasses.field( default=None ) input_terrain: dataclasses.InitVar[TerrainDescriptor] = dataclasses.field( @@ -32,9 +32,15 @@ class ContactPointStatePlotterSettings: ) def __post_init__( - self, input_axes: matplotlib.axes.Axes, input_terrain: TerrainDescriptor + self, input_axes: list[matplotlib.axes.Axes], input_terrain: TerrainDescriptor ): - self.axes = input_axes + self.axes = None + if isinstance(input_axes, list): + if len(input_axes) != 2: + raise ValueError("input_axes must be a list of length 2.") + + self.axes = input_axes + self.terrain = ( input_terrain if isinstance(input_terrain, TerrainDescriptor) @@ -68,17 +74,22 @@ def plot_complementarity( ) if self._axes is None: - self._fig, self._axes = plt.subplots() + self._fig, self._axes = plt.subplots(nrows=1, ncols=2) + plt.tight_layout() height_function = self.settings.terrain.height_function() normal_direction_fun = self.settings.terrain.normal_direction_function() positions = np.array([height_function(s.p) for s in states]).flatten() forces = np.array([normal_direction_fun(s.p).T @ s.f for s in states]).flatten() - self._axes.plot(_time_s, positions) - self._axes.set_ylabel("Height [m]", color="C0") - self._axes.tick_params(axis="y", color="C0", labelcolor="C0") - axes_force = self._axes.twinx() + complementarity_error = np.multiply(positions, forces) + self._axes[1].plot(_time_s, complementarity_error) + self._axes[1].set_ylabel("Complementarity Error [Nm]") + self._axes[1].set_xlabel("Time [s]") + self._axes[0].plot(_time_s, positions) + self._axes[0].set_ylabel("Height [m]", color="C0") + self._axes[0].tick_params(axis="y", color="C0", labelcolor="C0") + axes_force = self._axes[0].twinx() axes_force.plot(_time_s, forces, "C1") axes_force.set_ylabel("Normal Force [N]", color="C1") axes_force.tick_params(axis="y", color="C1", labelcolor="C1") @@ -164,9 +175,10 @@ def _create_complementarity_plot( number_of_columns: int, terrain: TerrainDescriptor, ): - number_of_plots = len(states[0]) + number_of_points = len(states[0]) + number_of_plots = number_of_points + 1 _number_of_columns = ( - math.ceil(math.sqrt(number_of_plots)) + math.floor(math.sqrt(number_of_plots)) if number_of_columns < 1 else number_of_columns ) @@ -177,16 +189,23 @@ def _create_complementarity_plot( ncols=_number_of_columns, squeeze=False, ) + plt.tight_layout() + last_plot_column = number_of_points - _number_of_columns * (number_of_rows - 1) + last_plot = axes_list[number_of_rows - 1][last_plot_column] _point_plotters = [ ContactPointStatePlotter( - ContactPointStatePlotterSettings(input_axes=el, terrain=terrain) + ContactPointStatePlotterSettings( + input_axes=[el, last_plot], + terrain=terrain, + ) ) for row in axes_list for el in row ] - assert len(_point_plotters) == number_of_plots + for i in range(last_plot_column + 1, _number_of_columns): + axes_list[number_of_rows - 1][i].remove() - for p in range(number_of_plots): + for p in range(number_of_points): contact_states = [state[p] for state in states] _point_plotters[p].plot_complementarity( states=contact_states, time_s=time_s From fa30b72077ae076b99981e5307888137aaec7796 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 31 Oct 2023 19:48:17 +0100 Subject: [PATCH 057/106] Added possibility to force close the complementarity plots --- .../robot_planning/utilities/foot_contact_state_plotter.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py index 919c3ad1..abd3560f 100644 --- a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -215,3 +215,9 @@ def _create_complementarity_plot( plt.draw() plt.pause(0.001) plt.show() + + def close(self): + if self._ext_process is not None: + self._ext_process.terminate() + self._ext_process = None + plt.close("all") From fd47e92c3f3ee2db61e3ca3fa106a15bc47c3f14 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 2 Nov 2023 18:40:52 +0100 Subject: [PATCH 058/106] Added possibility to plot the contact forces --- .../utilities/foot_contact_state_plotter.py | 239 ++++++++++++++---- 1 file changed, 193 insertions(+), 46 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py index abd3560f..f1ab3d17 100644 --- a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -3,7 +3,7 @@ import logging import math import multiprocessing -from typing import TypeVar +from typing import Callable, TypeVar import matplotlib.axes import matplotlib.pyplot as plt @@ -21,10 +21,16 @@ @dataclasses.dataclass class ContactPointStatePlotterSettings: - axes: list[matplotlib.axes.Axes] | None = dataclasses.field(default=None) + complementarity_axes: list[matplotlib.axes.Axes] | None = dataclasses.field( + default=None + ) + force_axes: matplotlib.axes.Axes | None = dataclasses.field(default=None) terrain: TerrainDescriptor = dataclasses.field(default=None) - input_axes: dataclasses.InitVar[list[matplotlib.axes.Axes]] = dataclasses.field( + input_complementarity_axes: dataclasses.InitVar[ + list[matplotlib.axes.Axes] + ] = dataclasses.field(default=None) + input_force_axes: dataclasses.InitVar[matplotlib.axes.Axes] = dataclasses.field( default=None ) input_terrain: dataclasses.InitVar[TerrainDescriptor] = dataclasses.field( @@ -32,14 +38,23 @@ class ContactPointStatePlotterSettings: ) def __post_init__( - self, input_axes: list[matplotlib.axes.Axes], input_terrain: TerrainDescriptor + self, + input_complementarity_axes: list[matplotlib.axes.Axes], + input_force_axes: matplotlib.axes.Axes, + input_terrain: TerrainDescriptor, ): - self.axes = None - if isinstance(input_axes, list): - if len(input_axes) != 2: + self.complementarity_axes = None + if isinstance(input_complementarity_axes, list): + if len(input_complementarity_axes) != 2: raise ValueError("input_axes must be a list of length 2.") - self.axes = input_axes + self.complementarity_axes = input_complementarity_axes + + self.force_axes = ( + input_force_axes + if isinstance(input_force_axes, matplotlib.axes.Axes) + else None + ) self.terrain = ( input_terrain @@ -54,15 +69,17 @@ def __init__( settings: ContactPointStatePlotterSettings = ContactPointStatePlotterSettings(), ): self.settings = settings - self._axes = self.settings.axes - self._fig = None + self._complementarity_axes = self.settings.complementarity_axes + self._complementarity_fig = None + self._force_axes = self.settings.force_axes + self._force_fig = None def plot_complementarity( self, states: list[ContactPointState], time_s: float | list[float] | np.ndarray = None, title: str = "Contact Point Complementarity", - ): + ) -> None: _time_s = copy.deepcopy(time_s) if _time_s is None or isinstance(_time_s, float) or _time_s.size == 1: single_step = _time_s if _time_s is not None else 0.0 @@ -73,8 +90,10 @@ def plot_complementarity( "timestep_s and foot_contact_states have different lengths." ) - if self._axes is None: - self._fig, self._axes = plt.subplots(nrows=1, ncols=2) + if self._complementarity_axes is None: + self._complementarity_fig, self._complementarity_axes = plt.subplots( + nrows=1, ncols=2 + ) plt.tight_layout() height_function = self.settings.terrain.height_function() @@ -83,21 +102,53 @@ def plot_complementarity( positions = np.array([height_function(s.p) for s in states]).flatten() forces = np.array([normal_direction_fun(s.p).T @ s.f for s in states]).flatten() complementarity_error = np.multiply(positions, forces) - self._axes[1].plot(_time_s, complementarity_error) - self._axes[1].set_ylabel("Complementarity Error [Nm]") - self._axes[1].set_xlabel("Time [s]") - self._axes[0].plot(_time_s, positions) - self._axes[0].set_ylabel("Height [m]", color="C0") - self._axes[0].tick_params(axis="y", color="C0", labelcolor="C0") - axes_force = self._axes[0].twinx() + self._complementarity_axes[1].plot(_time_s, complementarity_error) + self._complementarity_axes[1].set_ylabel("Complementarity Error [Nm]") + self._complementarity_axes[1].set_xlabel("Time [s]") + self._complementarity_axes[0].plot(_time_s, positions) + self._complementarity_axes[0].set_ylabel("Height [m]", color="C0") + self._complementarity_axes[0].tick_params(axis="y", color="C0", labelcolor="C0") + self._complementarity_axes[0].set_xlabel("Time [s]") + axes_force = self._complementarity_axes[0].twinx() axes_force.plot(_time_s, forces, "C1") axes_force.set_ylabel("Normal Force [N]", color="C1") axes_force.tick_params(axis="y", color="C1", labelcolor="C1") axes_force.spines["right"].set_color("C1") axes_force.spines["left"].set_color("C0") - if self._fig is not None: - self._fig.suptitle(title) + if self._complementarity_fig is not None: + self._complementarity_fig.suptitle(title) + plt.draw() + plt.pause(0.001) + plt.show() + + def plot_forces( + self, + states: list[ContactPointState], + time_s: float | list[float] | np.ndarray = None, + title: str = "Contact Point Forces", + ) -> None: + _time_s = copy.deepcopy(time_s) + if _time_s is None or isinstance(_time_s, float) or _time_s.size == 1: + single_step = _time_s if _time_s is not None else 0.0 + _time_s = np.linspace(0, len(states) * single_step, len(states)) + + if len(_time_s) != len(states): + raise ValueError( + "timestep_s and foot_contact_states have different lengths." + ) + + if self._force_axes is None: + self._force_fig, self._force_axes = plt.subplots(nrows=1, ncols=1) + plt.tight_layout() + + forces = np.array([s.f for s in states]) + self._force_axes.plot(_time_s, forces) + self._force_axes.set_ylabel("Force [N]") + self._force_axes.set_xlabel("Time [s]") + + if self._force_fig is not None: + self._force_fig.suptitle(title) plt.draw() plt.pause(0.001) plt.show() @@ -120,23 +171,20 @@ def __init__( settings: FootContactStatePlotterSettings = FootContactStatePlotterSettings(), ): self._settings = settings - self._ext_process = None + self._ext_proc_complementarity = None + self._ext_proc_forces = None self._logger = logging.getLogger("[hippopt::FootContactStatePlotter]") - def plot_complementarity( + def _plot( self, states: list[FootContactState], - time_s: float | list[float] | np.ndarray = None, - title: str = "Foot Contact Complementarity", - blocking: bool = False, - ): - if self._ext_process is not None: - self._logger.warning( - "A plot is already running. " - "Make sure to close the previous plot first." - ) - self._ext_process.join() - self._ext_process = None + time_s: float | list[float] | np.ndarray, + title: str, + blocking: bool, + plot_function: Callable[ + [list[FootContactState], np.ndarray, str, int, TerrainDescriptor], None + ], + ) -> multiprocessing.Process | None: _time_s = copy.deepcopy(time_s) _states = copy.deepcopy(states) _terrain = copy.deepcopy(self._settings.terrain) @@ -150,10 +198,10 @@ def plot_complementarity( ) if len(_states) == 0: - return + return None - self._ext_process = multiprocessing.Process( - target=FootContactStatePlotter._create_complementarity_plot, + process = multiprocessing.Process( + target=plot_function, args=( _states, _time_s, @@ -162,10 +210,13 @@ def plot_complementarity( _terrain, ), ) - self._ext_process.start() + process.start() if blocking: - self._ext_process.join() + process.join() + process = None + + return process @staticmethod def _create_complementarity_plot( @@ -174,7 +225,7 @@ def _create_complementarity_plot( title: str, number_of_columns: int, terrain: TerrainDescriptor, - ): + ) -> None: number_of_points = len(states[0]) number_of_plots = number_of_points + 1 _number_of_columns = ( @@ -195,7 +246,7 @@ def _create_complementarity_plot( _point_plotters = [ ContactPointStatePlotter( ContactPointStatePlotterSettings( - input_axes=[el, last_plot], + input_complementarity_axes=[el, last_plot], terrain=terrain, ) ) @@ -216,8 +267,104 @@ def _create_complementarity_plot( plt.pause(0.001) plt.show() - def close(self): - if self._ext_process is not None: - self._ext_process.terminate() - self._ext_process = None + @staticmethod + def _create_forces_plot( + states: list[FootContactState], + time_s: np.ndarray, + title: str, + number_of_columns: int, + _: TerrainDescriptor, + ) -> None: + number_of_points = len(states[0]) + number_of_plots = number_of_points + _number_of_columns = ( + math.floor(math.sqrt(number_of_plots)) + if number_of_columns < 1 + else number_of_columns + ) + number_of_rows = math.ceil(number_of_plots / _number_of_columns) + + _fig, axes_list = plt.subplots( + nrows=number_of_rows, + ncols=_number_of_columns, + squeeze=False, + ) + plt.tight_layout() + last_plot_column = ( + number_of_points - 1 - _number_of_columns * (number_of_rows - 1) + ) + _point_plotters = [ + ContactPointStatePlotter( + ContactPointStatePlotterSettings( + input_force_axes=el, + ) + ) + for row in axes_list + for el in row + ] + for i in range(last_plot_column + 1, _number_of_columns): + axes_list[number_of_rows - 1][i].remove() + + for p in range(number_of_points): + contact_states = [state[p] for state in states] + _point_plotters[p].plot_forces(states=contact_states, time_s=time_s) + + _fig.suptitle(title) + plt.draw() + plt.pause(0.001) + plt.show() + + def plot_complementarity( + self, + states: list[FootContactState], + time_s: float | list[float] | np.ndarray = None, + title: str = "Foot Contact Complementarity", + blocking: bool = False, + ) -> None: + if self._ext_proc_complementarity is not None: + self._logger.warning( + "A complementarity plot is already running. " + "Make sure to close the previous plot first." + ) + self._ext_proc_complementarity.join() + self._ext_proc_complementarity = None + + self._ext_proc_complementarity = self._plot( + states, + time_s, + title, + blocking, + self._create_complementarity_plot, + ) + + def plot_forces( + self, + states: list[FootContactState], + time_s: float | list[float] | np.ndarray = None, + title: str = "Foot Contact Forces", + blocking: bool = False, + ) -> None: + if self._ext_proc_forces is not None: + self._logger.warning( + "A force plot is already running. " + "Make sure to close the previous plot first." + ) + self._ext_proc_forces.join() + self._ext_proc_forces = None + + self._ext_proc_forces = self._plot( + states, + time_s, + title, + blocking, + self._create_forces_plot, + ) + + def close(self) -> None: + if self._ext_proc_complementarity is not None: + self._ext_proc_complementarity.terminate() + self._ext_proc_complementarity = None + if self._ext_proc_forces is not None: + self._ext_proc_forces.terminate() + self._ext_proc_forces = None plt.close("all") From 3a8fa35bbec938f071c89ad83b8569287dfe0224 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 2 Nov 2023 19:02:43 +0100 Subject: [PATCH 059/106] Create the frames folder only if saving --- .../robot_planning/utilities/humanoid_state_visualizer.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index d6625aa5..b4019bf4 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -303,9 +303,9 @@ def _visualize_multiple_states( raise ValueError("timestep_s and states have different lengths.") folder_name = f"{self._settings.working_folder}/{file_name_stem}_frames" - pathlib.Path(folder_name).mkdir(parents=True, exist_ok=True) if save: + pathlib.Path(folder_name).mkdir(parents=True, exist_ok=True) self._logger.info( f"Saving visualization frames in {folder_name}. " "Make sure to have the visualizer open, " From 0869c6c387808a0102923aa1c39eb93aec9b107c Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 10:07:38 +0100 Subject: [PATCH 060/106] Fixed visualization of forces --- .../utilities/humanoid_state_visualizer.py | 48 +++++++++++-------- 1 file changed, 27 insertions(+), 21 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index b4019bf4..1ec1005b 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -249,39 +249,45 @@ def _visualize_single_state( f"p_{i}", ) + point_force = point.f * self._settings.contact_force_scaling + # Copied from https://github.com/robotology/idyntree/pull/1087 until it is # available in conda - force_norm = np.linalg.norm(point.f) - - if force_norm < 1e-6: - rotation = np.eye(3) - else: - force_direction = point.f / force_norm - cos_angle = np.dot(np.array([0, 0, 1]), force_direction) - rotation_axis = self._skew(np.array([0, 0, 1])) @ force_direction - skew_symmetric_matrix = self._skew(rotation_axis) - rotation = ( - np.eye(3) - + skew_symmetric_matrix - + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) - * ((1 - cos_angle) / (force_norm**2)) - ) - - scaling = np.diag([1, 1, self._settings.contact_force_scaling * force_norm]) - position = ( - point.p + point.f * self._settings.contact_force_scaling / 2 - ) # the origin is in the cylinder center + position = point.p + point_force / 2 # the origin is in the cylinder center transform = np.zeros((4, 4)) transform[0:3, 3] = position transform[3, 3] = 1 - transform[0:3, 0:3] = rotation @ scaling + transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) self._viz.viewer[f"f_{i}"].set_transform(transform) if save: image = self._viz.viewer.get_image() image.save(file_name_stem + ".png") + def _get_force_scaled_rotation(self, point_force: np.ndarray) -> np.ndarray: + force_norm = np.linalg.norm(point_force) + scaling = np.diag([1, 1, force_norm]) + + if force_norm < 1e-6: + return scaling + + force_direction = point_force / force_norm + cos_angle = np.dot(np.array([0, 0, 1]), force_direction) + rotation_axis = self._skew(np.array([0, 0, 1])) @ force_direction + + if np.linalg.norm(rotation_axis) < 1e-6: + return scaling + + skew_symmetric_matrix = self._skew(rotation_axis) + rotation = ( + np.eye(3) + + skew_symmetric_matrix + + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) + * ((1 - cos_angle) / (np.linalg.norm(rotation_axis) ** 2)) + ) + return rotation @ scaling + def _visualize_multiple_states( self, states: list[HumanoidState], From d2cfcba095e390ca87743dbef17a9865b46ba5f8 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 17:55:14 +0100 Subject: [PATCH 061/106] Added possibility to specify the terrain name --- .../robot_planning/utilities/terrain_descriptor.py | 13 +++++++++++-- test/test_planar_terrain.py | 5 +++++ 2 files changed, 16 insertions(+), 2 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index 6608ea15..fe16905e 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -10,20 +10,26 @@ class TerrainDescriptor(abc.ABC): _normal_direction_function: cs.Function = dataclasses.field(default=None) _orientation_function: cs.Function = dataclasses.field(default=None) _point_position_name: str = dataclasses.field(default="point_position") + _name: str = dataclasses.field(default="terrain") _options: dict = dataclasses.field(default=None) point_position_name: dataclasses.InitVar[str] = dataclasses.field( default="point_position" ) options: dataclasses.InitVar[dict] = dataclasses.field(default=None) + name: dataclasses.InitVar[str] = dataclasses.field(default=None) def __post_init__( - self, point_position_name: str = "point_position", options: dict = None + self, + point_position_name: str = "point_position", + options: dict = None, + name: str = None, ): self.change_options(point_position_name, options) + self._name = name def change_options( self, point_position_name: str = "point_position", options: dict = None, **_ - ): + ) -> None: self._options = {} if options is None else options self._point_position_name = point_position_name @@ -60,6 +66,9 @@ def orientation_function(self) -> cs.Function: def get_point_position_name(self) -> str: return self._point_position_name + def get_name(self) -> str: + return self._name if isinstance(self._name, str) else self.__class__.__name__ + class PlanarTerrain(TerrainDescriptor): def create_height_function(self) -> cs.Function: diff --git a/test/test_planar_terrain.py b/test/test_planar_terrain.py index 87d10449..4ac8a0ba 100644 --- a/test/test_planar_terrain.py +++ b/test/test_planar_terrain.py @@ -31,3 +31,8 @@ def test_planar_terrain(): output = orientation_fun(dummy_point).full() assert (expected_orientation == output).all() # noqa + + assert planar_terrain.get_name() == "PlanarTerrain" + + other = hippopt.robot_planning.PlanarTerrain(name="other") + assert other.get_name() == "other" From f49e47a6fac5791c7f386a203144d1342b329ff1 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 18:15:50 +0100 Subject: [PATCH 062/106] Using the terrain name in the humanoid visualizer and avoid to overwrite if already present --- .../utilities/humanoid_state_visualizer.py | 41 ++++++++++++++----- 1 file changed, 30 insertions(+), 11 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 1ec1005b..b4ce7449 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -37,6 +37,7 @@ class HumanoidStateVisualizerSettings: ground_mesh_axis_points: int = dataclasses.field(default=None) ground_x_limits: list[float] = dataclasses.field(default=None) ground_y_limits: list[float] = dataclasses.field(default=None) + overwrite_ground_files: bool = dataclasses.field(default=False) def __post_init__(self): self.robot_color = [1, 1, 1, 0.25] @@ -53,7 +54,7 @@ def __post_init__(self): self.ground_mesh_axis_points = 200 self.working_folder = "./" - def is_valid(self): + def is_valid(self) -> bool: ok = True logger = logging.getLogger("[hippopt::HumanoidStateVisualizerSettings]") if self.robot_model is None: @@ -127,7 +128,10 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: color=settings.robot_color, ) self._viz.load_model_from_file( - model_path=os.path.join(self._settings.working_folder, "ground.urdf"), + model_path=os.path.join( + self._settings.working_folder, + self._settings.terrain.get_name() + ".urdf", + ), model_name="ground", color=settings.ground_color, ) @@ -149,8 +153,17 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: color=settings.contact_forces_color, ) - def _create_ground_urdf(self): - with open(os.path.join(self._settings.working_folder, "ground.urdf"), "w") as f: + def _create_ground_urdf(self) -> None: + filename = self._settings.terrain.get_name() + ".urdf" + full_filename = os.path.join(self._settings.working_folder, filename) + if os.path.exists(full_filename): + if self._settings.overwrite_ground_files: + self._logger.info(f"Overwriting {filename}") + else: + self._logger.info(f"{filename} already exists. Skipping creation.") + return + + with open(full_filename, "w") as f: f.write( """ @@ -187,7 +200,15 @@ def _create_ground_urdf(self): """ ) - def _create_ground_mesh(self): + def _create_ground_mesh(self) -> None: + filename = self._settings.terrain.get_name() + ".stl" + full_filename = os.path.join(self._settings.working_folder, filename) + if os.path.exists(full_filename): + if self._settings.overwrite_ground_files: + self._logger.info(f"Overwriting {filename}") + else: + self._logger.info(f"{filename} already exists. Skipping creation.") + return x_step = ( self._settings.ground_x_limits[1] - self._settings.ground_x_limits[0] ) / self._settings.ground_mesh_axis_points @@ -209,9 +230,7 @@ def _create_ground_mesh(self): points = np.array([x.flatten(), y.flatten(), np.zeros(x.size)]) height_function_map = self._settings.terrain.height_function().map(x.size) z = -np.array(height_function_map(points).full()).reshape(x.shape) - surf2stl.write( - os.path.join(self._settings.working_folder, "ground.stl"), x, y, z - ) + surf2stl.write(full_filename, x, y, z) @staticmethod def _skew(x: np.ndarray) -> np.ndarray: @@ -225,7 +244,7 @@ def _skew(x: np.ndarray) -> np.ndarray: def _visualize_single_state( self, state: HumanoidState, save: bool, file_name_stem: str - ): + ) -> None: self._viz.set_multibody_system_state( state.kinematics.base.position, liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) @@ -295,7 +314,7 @@ def _visualize_multiple_states( time_multiplier: float, save: bool, file_name_stem: str, - ): + ) -> None: _timestep_s = copy.deepcopy(timestep_s) if ( _timestep_s is None @@ -376,7 +395,7 @@ def visualize( time_multiplier: float = 1.0, save: bool = False, file_name_stem: str = "humanoid_state_visualization", - ): + ) -> None: if isinstance(state, list): self._visualize_multiple_states( state, From 4ae37190a96bba9b7c3d037000477bc99e2f9b6a Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 3 Nov 2023 19:24:15 +0100 Subject: [PATCH 063/106] Extracted function to create video from frames --- .../utilities/humanoid_state_visualizer.py | 30 +++++++++++++------ 1 file changed, 21 insertions(+), 9 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index b4ce7449..0e18ddf4 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -337,13 +337,15 @@ def _visualize_multiple_states( "otherwise the process will hang." ) + frame_prefix = "frame_" + for i, state in enumerate(states): self._logger.info(f"Visualizing state {i + 1}/{len(states)}") start = time.time() self._visualize_single_state( state, save=save, - file_name_stem=f"{folder_name}/frame_{i:03}", + file_name_stem=f"{folder_name}/{frame_prefix}{i:03}", ) end = time.time() elapsed_s = end - start @@ -378,15 +380,25 @@ def _visualize_multiple_states( self._logger.warning("Using the fps=1.0") fps = 1.0 - frames = ffmpeg.input( - f"{folder_name}/frame_*.png", pattern_type="glob", framerate=fps - ) - video = ffmpeg.output( - frames, - f"{self._settings.working_folder}/{file_name_stem}.mp4", - video_bitrate="20M", + self.generate_video_from_frames( + file_name_stem=file_name_stem, + frames_folder=folder_name, + frames_prefix=frame_prefix, + fps=fps, ) - ffmpeg.run(video) + + def generate_video_from_frames( + self, file_name_stem: str, frames_folder: str, frames_prefix: str, fps: float + ) -> None: + frames = ffmpeg.input( + f"{frames_folder}/{frames_prefix}*.png", pattern_type="glob", framerate=fps + ) + video = ffmpeg.output( + frames, + f"{self._settings.working_folder}/{file_name_stem}.mp4", + video_bitrate="20M", + ) + ffmpeg.run(video) def visualize( self, From 96c85bba5d9b818a90f8edd0d90acd552bbb8475 Mon Sep 17 00:00:00 2001 From: Stefano Date: Sat, 4 Nov 2023 19:07:33 +0100 Subject: [PATCH 064/106] Added possibility to visualize multiple states at once --- .../utilities/humanoid_state_visualizer.py | 252 +++++++++++------- 1 file changed, 162 insertions(+), 90 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 0e18ddf4..6bbbe13d 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -37,7 +37,8 @@ class HumanoidStateVisualizerSettings: ground_mesh_axis_points: int = dataclasses.field(default=None) ground_x_limits: list[float] = dataclasses.field(default=None) ground_y_limits: list[float] = dataclasses.field(default=None) - overwrite_ground_files: bool = dataclasses.field(default=False) + overwrite_ground_files: bool = dataclasses.field(default=None) + pre_allocated_clones: int = dataclasses.field(default=None) def __post_init__(self): self.robot_color = [1, 1, 1, 0.25] @@ -53,6 +54,8 @@ def __post_init__(self): self.ground_y_limits = [-1.5, 1.5] self.ground_mesh_axis_points = 200 self.working_folder = "./" + self.overwrite_ground_files = False + self.pre_allocated_clones = 1 def is_valid(self) -> bool: ok = True @@ -109,6 +112,12 @@ def is_valid(self) -> bool: ): logger.error("ground_y_limits are not specified correctly.") ok = False + if self.contact_force_scaling <= 0: + logger.error("contact_force_scaling is not specified correctly.") + ok = False + if self.pre_allocated_clones <= 0: + logger.error("pre_allocated_clones is not specified correctly.") + ok = False return ok @@ -120,39 +129,130 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: self._settings = settings self._create_ground_urdf() self._create_ground_mesh() + self._number_of_clones = self._settings.pre_allocated_clones self._viz = MeshcatVisualizer() - self._viz.load_model_from_file( - model_path=settings.robot_model, - model_name="robot", - considered_joints=settings.considered_joints, - color=settings.robot_color, - ) self._viz.load_model_from_file( model_path=os.path.join( self._settings.working_folder, self._settings.terrain.get_name() + ".urdf", ), model_name="ground", - color=settings.ground_color, + color=self._settings.ground_color, + ) + for i in range(self._number_of_clones): + self._allocate_clone(i) + if i != 0: + self._set_clone_visibility(i, False) + + def _allocate_clone(self, index: int) -> None: + self._viz.load_model_from_file( + model_path=self._settings.robot_model, + model_name=f"[{index}]robot", + considered_joints=self._settings.considered_joints, + color=self._settings.robot_color, ) self._viz.load_sphere( - radius=settings.com_radius, shape_name="CoM", color=settings.com_color + radius=self._settings.com_radius, + shape_name=f"[{index}]CoM", + color=self._settings.com_color, ) for i, point in enumerate( - (settings.contact_points.left + settings.contact_points.right) + (self._settings.contact_points.left + self._settings.contact_points.right) ): self._viz.load_sphere( - radius=settings.contact_points_radius, - shape_name=f"p_{i}", - color=settings.contact_points_color, + radius=self._settings.contact_points_radius, + shape_name=f"[{index}]p_{i}", + color=self._settings.contact_points_color, ) self._viz.load_cylinder( - radius=settings.contact_force_radius, + radius=self._settings.contact_force_radius, length=1.0, - shape_name=f"f_{i}", - color=settings.contact_forces_color, + shape_name=f"[{index}]f_{i}", + color=self._settings.contact_forces_color, + ) + + def _set_clone_visibility(self, index: int, visible: bool) -> None: + self._viz.viewer[f"[{index}]robot"].set_property(key="visible", value=visible) + self._viz.viewer[f"[{index}]CoM"].set_property(key="visible", value=visible) + for i, point in enumerate( + (self._settings.contact_points.left + self._settings.contact_points.right) + ): + self._viz.viewer[f"[{index}]p_{i}"].set_property( + key="visible", value=visible + ) + self._viz.viewer[f"[{index}]f_{i}"].set_property( + key="visible", value=visible + ) + + @staticmethod + def _skew(x: np.ndarray) -> np.ndarray: + return np.array( + [ + [0, -x[2], x[1]], + [x[2], 0, -x[0]], + [-x[1], x[0], 0], + ] + ) + + def _get_force_scaled_rotation(self, point_force: np.ndarray) -> np.ndarray: + force_norm = np.linalg.norm(point_force) + scaling = np.diag([1, 1, force_norm]) + + if force_norm < 1e-6: + return scaling + + force_direction = point_force / force_norm + cos_angle = np.dot(np.array([0, 0, 1]), force_direction) + rotation_axis = self._skew(np.array([0, 0, 1])) @ force_direction + + if np.linalg.norm(rotation_axis) < 1e-6: + return scaling + + skew_symmetric_matrix = self._skew(rotation_axis) + rotation = ( + np.eye(3) + + skew_symmetric_matrix + + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) + * ((1 - cos_angle) / (np.linalg.norm(rotation_axis) ** 2)) + ) + return rotation @ scaling + + def _update_clone(self, index: int, state: HumanoidState) -> None: + self._viz.set_multibody_system_state( + state.kinematics.base.position, + liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) + .as_matrix() + .full(), + state.kinematics.joints.positions, + f"[{index}]robot", + ) + self._viz.set_primitive_geometry_transform( + state.com, + np.eye(3), + f"[{index}]CoM", + ) + + for i, point in enumerate( + (state.contact_points.left + state.contact_points.right) + ): + self._viz.set_primitive_geometry_transform( + point.p, + np.eye(3), + f"[{index}]p_{i}", ) + point_force = point.f * self._settings.contact_force_scaling + + # Copied from https://github.com/robotology/idyntree/pull/1087 until it is + # available in conda + + position = point.p + point_force / 2 # the origin is in the cylinder center + transform = np.zeros((4, 4)) + transform[0:3, 3] = position + transform[3, 3] = 1 + transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) + self._viz.viewer[f"[{index}]f_{i}"].set_transform(transform) + def _create_ground_urdf(self) -> None: filename = self._settings.terrain.get_name() + ".urdf" full_filename = os.path.join(self._settings.working_folder, filename) @@ -232,86 +332,40 @@ def _create_ground_mesh(self) -> None: z = -np.array(height_function_map(points).full()).reshape(x.shape) surf2stl.write(full_filename, x, y, z) - @staticmethod - def _skew(x: np.ndarray) -> np.ndarray: - return np.array( - [ - [0, -x[2], x[1]], - [x[2], 0, -x[0]], - [-x[1], x[0], 0], - ] - ) - def _visualize_single_state( - self, state: HumanoidState, save: bool, file_name_stem: str + self, + states: list[HumanoidState], + save: bool, + file_name_stem: str, ) -> None: - self._viz.set_multibody_system_state( - state.kinematics.base.position, - liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) - .as_matrix() - .full(), - state.kinematics.joints.positions, - "robot", - ) - self._viz.set_primitive_geometry_transform( - state.com, - np.eye(3), - "CoM", - ) - - for i, point in enumerate( - (state.contact_points.left + state.contact_points.right) - ): - self._viz.set_primitive_geometry_transform( - point.p, - np.eye(3), - f"p_{i}", + if len(states) > self._number_of_clones: + self._logger.warning( + f"Number of states ({len(states)}) is greater than the number of " + f"allocated clones ({self._number_of_clones}). " + "Creating new clones." ) + for i in range(self._number_of_clones, len(states)): + self._allocate_clone(i) + self._set_clone_visibility(i, False) + self._number_of_clones += 1 - point_force = point.f * self._settings.contact_force_scaling + for i in range(len(states)): + self._update_clone(i, states[i]) + self._set_clone_visibility(i, True) - # Copied from https://github.com/robotology/idyntree/pull/1087 until it is - # available in conda - - position = point.p + point_force / 2 # the origin is in the cylinder center - transform = np.zeros((4, 4)) - transform[0:3, 3] = position - transform[3, 3] = 1 - transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) - self._viz.viewer[f"f_{i}"].set_transform(transform) + for i in range(len(states), self._number_of_clones): + self._set_clone_visibility(i, False) if save: image = self._viz.viewer.get_image() image.save(file_name_stem + ".png") - def _get_force_scaled_rotation(self, point_force: np.ndarray) -> np.ndarray: - force_norm = np.linalg.norm(point_force) - scaling = np.diag([1, 1, force_norm]) - - if force_norm < 1e-6: - return scaling - - force_direction = point_force / force_norm - cos_angle = np.dot(np.array([0, 0, 1]), force_direction) - rotation_axis = self._skew(np.array([0, 0, 1])) @ force_direction - - if np.linalg.norm(rotation_axis) < 1e-6: - return scaling - - skew_symmetric_matrix = self._skew(rotation_axis) - rotation = ( - np.eye(3) - + skew_symmetric_matrix - + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) - * ((1 - cos_angle) / (np.linalg.norm(rotation_axis) ** 2)) - ) - return rotation @ scaling - def _visualize_multiple_states( self, states: list[HumanoidState], timestep_s: float | list[float] | np.ndarray, time_multiplier: float, + number_of_clones: int, save: bool, file_name_stem: str, ) -> None: @@ -339,11 +393,19 @@ def _visualize_multiple_states( frame_prefix = "frame_" - for i, state in enumerate(states): - self._logger.info(f"Visualizing state {i + 1}/{len(states)}") + for i in range(number_of_clones, len(states)): + initial_index = i - number_of_clones + visualized_states = states[initial_index:i] + if number_of_clones > 1: + self._logger.info( + f"Visualizing states [{i-number_of_clones + 1},{i + 1}]" + f" of {len(states)}." + ) + else: + self._logger.info(f"Visualizing state {i}/{len(states)}") start = time.time() self._visualize_single_state( - state, + visualized_states, save=save, file_name_stem=f"{folder_name}/{frame_prefix}{i:03}", ) @@ -384,14 +446,16 @@ def _visualize_multiple_states( file_name_stem=file_name_stem, frames_folder=folder_name, frames_prefix=frame_prefix, - fps=fps, + fps=fps / time_multiplier, ) def generate_video_from_frames( self, file_name_stem: str, frames_folder: str, frames_prefix: str, fps: float ) -> None: frames = ffmpeg.input( - f"{frames_folder}/{frames_prefix}*.png", pattern_type="glob", framerate=fps + filename=f"{frames_folder}/{frames_prefix}*.png", + pattern_type="glob", + framerate=fps, ) video = ffmpeg.output( frames, @@ -402,21 +466,29 @@ def generate_video_from_frames( def visualize( self, - state: HumanoidState | list[HumanoidState], + states: HumanoidState | list[HumanoidState], timestep_s: float | list[float] | np.ndarray = None, time_multiplier: float = 1.0, + number_of_clones: int = 1, save: bool = False, file_name_stem: str = "humanoid_state_visualization", ) -> None: - if isinstance(state, list): + if number_of_clones < 1: + raise ValueError("number_of_clones must be greater than 0.") + + if not isinstance(states, list): + states = [states] + + if number_of_clones < len(states): self._visualize_multiple_states( - state, + states=states, timestep_s=timestep_s, time_multiplier=time_multiplier, + number_of_clones=number_of_clones, save=save, file_name_stem=file_name_stem, ) else: self._visualize_single_state( - state, save=save, file_name_stem=file_name_stem + states=states, save=save, file_name_stem=file_name_stem ) From 62e045084f8474d9a4766db303e227b2259306ef Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 23 Nov 2023 17:17:14 +0100 Subject: [PATCH 065/106] Added method to convert from list to FootContactState --- src/hippopt/robot_planning/variables/contacts.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index b3197fd4..591349b8 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -1,4 +1,5 @@ import dataclasses +from typing import TypeVar import liecasadi import numpy as np @@ -90,6 +91,9 @@ def __post_init__(self) -> None: self.f_dot = np.zeros(3) +TFootContactState = TypeVar("TFootContactState", bound="FootContactState") + + class FootContactState(list[ContactPointState]): def set_from_parent_frame_transform(self, transform: liecasadi.SE3): for contact_point in self: @@ -97,10 +101,17 @@ def set_from_parent_frame_transform(self, transform: liecasadi.SE3): contact_point.descriptor.position_in_foot_frame ) + @staticmethod + def from_list(input_list: list[ContactPointState]) -> TFootContactState: + output = FootContactState() + for contact_point in input_list: + output.append(contact_point) + return output + @staticmethod def from_parent_frame_transform( descriptor: list[ContactPointDescriptor], transform: liecasadi.SE3 - ): + ) -> TFootContactState: foot_contact_state = FootContactState() for contact_point_descriptor in descriptor: foot_contact_state.append( From d710893e270f96ec5925dddbf94d5a4a7b787db3 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 24 Nov 2023 16:36:35 +0100 Subject: [PATCH 066/106] Turned FootContactState into an OptimizationObject --- src/hippopt/robot_planning/variables/contacts.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 591349b8..54b1b2a3 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -94,7 +94,8 @@ def __post_init__(self) -> None: TFootContactState = TypeVar("TFootContactState", bound="FootContactState") -class FootContactState(list[ContactPointState]): +@dataclasses.dataclass +class FootContactState(list[ContactPointState], OptimizationObject): def set_from_parent_frame_transform(self, transform: liecasadi.SE3): for contact_point in self: contact_point.p = transform.translation() + transform.rotation().act( From a9dae49bdfd6e548a73fdd24f41e60c7b24d86f4 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 24 Nov 2023 16:37:06 +0100 Subject: [PATCH 067/106] Added conversion from FloatingBaseSystem to FloatingBaseSystemState --- src/hippopt/robot_planning/variables/floating_base.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index e86e6b65..cc3fd154 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -138,3 +138,10 @@ class FloatingBaseSystem(OptimizationObject): def __post_init__(self, number_of_joints: int): self.joints = KinematicTree(number_of_joints_state=number_of_joints) + + def to_floating_base_system_state(self): + output = FloatingBaseSystemState() + output.base.position = self.base.position + output.base.quaternion_xyzw = self.base.quaternion_xyzw + output.joints.positions = self.joints.positions + return output From f1943ccc7d370abc917a9f678449cda3faf34bb7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 24 Nov 2023 18:16:39 +0100 Subject: [PATCH 068/106] Removed centroidal momentum from the HumanoidState --- src/hippopt/robot_planning/variables/humanoid.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/hippopt/robot_planning/variables/humanoid.py b/src/hippopt/robot_planning/variables/humanoid.py index 60db2a8b..6b732221 100644 --- a/src/hippopt/robot_planning/variables/humanoid.py +++ b/src/hippopt/robot_planning/variables/humanoid.py @@ -29,7 +29,6 @@ class HumanoidState(OptimizationObject): ) com: StorageType = default_storage_field(OverridableVariable) - centroidal_momentum: StorageType = default_storage_field(OverridableVariable) contact_point_descriptors: dataclasses.InitVar[ FeetContactPointDescriptors @@ -55,4 +54,3 @@ def __post_init__( number_of_joints_state=number_of_joints ) self.com = np.zeros(3) - self.centroidal_momentum = np.zeros(6) From 31d2c8f33455dcb9c03550bdaf6ccb6ac2432070 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 11:59:34 +0100 Subject: [PATCH 069/106] Added possibility to move from HumanoidState to ExtendedhumanoidState --- .../robot_planning/variables/floating_base.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index cc3fd154..f3084e1b 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -1,4 +1,5 @@ import dataclasses +from typing import TypeVar import numpy as np @@ -125,6 +126,9 @@ def __post_init__(self, number_of_joints_derivative: int): ) +TFloatingBaseSystem = TypeVar("TFloatingBaseSystem", bound="FloatingBaseSystem") + + @dataclasses.dataclass class FloatingBaseSystem(OptimizationObject): base: CompositeType[FreeFloatingObject] = default_composite_field( @@ -145,3 +149,16 @@ def to_floating_base_system_state(self): output.base.quaternion_xyzw = self.base.quaternion_xyzw output.joints.positions = self.joints.positions return output + + @staticmethod + def from_floating_base_system_state( + state: FloatingBaseSystemState, + ) -> TFloatingBaseSystem: + output = FloatingBaseSystem(number_of_joints=len(state.joints.positions)) + output.base.position = state.base.position + output.base.quaternion_xyzw = state.base.quaternion_xyzw + output.base.linear_velocity = None + output.base.quaternion_velocity_xyzw = None + output.joints.positions = state.joints.positions + output.joints.velocities = None + return output From e72646c93fd64550349ab4b600c5854cbbcedd45 Mon Sep 17 00:00:00 2001 From: Stefano Date: Mon, 27 Nov 2023 18:01:57 +0100 Subject: [PATCH 070/106] Added first implementation of interpolators to define initial guess --- .../robot_planning/utilities/interpolators.py | 196 ++++++++++++++++++ .../robot_planning/variables/contacts.py | 20 ++ 2 files changed, 216 insertions(+) create mode 100644 src/hippopt/robot_planning/utilities/interpolators.py diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py new file mode 100644 index 00000000..12abd172 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -0,0 +1,196 @@ +import casadi as cs +import liecasadi +import numpy as np + +from hippopt import StorageType +from hippopt.robot_planning.variables.contacts import ( + ContactPointDescriptor, + FootContactPhaseDescriptor, + FootContactState, +) + + +def linear_interpolator( + initial: StorageType, final: StorageType, number_of_points: int +) -> list[StorageType]: + assert not isinstance(initial, list) and not isinstance(final, list) + + interpolator = cs.interpolant("lerp", "linear", [initial, final], [0.0, 1.0]) + x = np.linspace(start=0.0, stop=1.0, num=number_of_points) + return [interpolator(x_i) for x_i in x] + + +def quaternion_slerp( + initial: StorageType, final: StorageType, number_of_points: int +) -> list[StorageType]: + assert not isinstance(initial, list) and not isinstance(final, list) + + x = np.linspace(start=0.0, stop=1.0, num=number_of_points) + return [liecasadi.Quaternion.slerp_step(initial, final, t) for t in x] + + +def transform_interpolator( + initial: liecasadi.SE3, final: liecasadi.SE3, number_of_points: int +) -> list[liecasadi.SE3]: + linear_interpolation = linear_interpolator( + initial=initial.translation(), + final=final.translation(), + number_of_points=number_of_points, + ) + quaternion_interpolation = quaternion_slerp( + initial=initial.rotation(), + final=final.rotation(), + number_of_points=number_of_points, + ) + output = [] + for i in range(number_of_points): + output.append( + liecasadi.SE3(quaternion_interpolation[i], linear_interpolation[i]) + ) + return output + + +def foot_contact_state_interpolator( + phases: list[FootContactPhaseDescriptor], + descriptor: list[ContactPointDescriptor], + number_of_points: int, + dt: float, + t0: float = 0.0, +) -> list[FootContactState]: + assert len(phases) > 0 + assert number_of_points > 0 + assert dt > 0.0 + + end_time = t0 + dt * number_of_points + + if phases[0].activation_time is None: + deactivation_time = ( + phases[0].deactivation_time + if phases[0].deactivation_time is not None + else t0 + ) + phases[0].activation_time = min(deactivation_time, t0) - dt + + for i, phase in enumerate(phases): + if phase.activation_time is None: + raise ValueError( + f"Phase {i} has no activation time, but is not the first phase." + ) + + last = len(phases) - 1 + if phases[last].deactivation_time is None: + phases[last].deactivation_time = ( + max(end_time, phases[last].activation_time) + dt + ) + + if phases[last].deactivation_time < end_time: + raise ValueError( + f"The Last phase deactivation time " + f"({phases[len(phases) - 1].deactivation_time}) is before " + f"the end time ({end_time}, computed from the inputs)." + ) + + for i, phase in enumerate(phases): + if phase.deactivation_time is None: + raise ValueError( + f"Phase {i} has no deactivation time, but is not the last phase." + ) + if phase.activation_time > phase.deactivation_time: + raise ValueError( + f"Phase {i} has an activation time ({phase.activation_time}) " + f"greater than its deactivation time ({phase.deactivation_time})." + ) + + if i < last: + if phase.deactivation_time > phases[i + 1].activation_time: + raise ValueError( + f"Phase {i} has a deactivation time ({phase.deactivation_time}) " + f"greater than the activation time of the next phase " + f"({phases[i + 1].activation_time})." + ) + + output = [] + + def append_stance_phase( + stance_phase: FootContactPhaseDescriptor, + points: int, + ) -> None: + for _ in range(points): + foot_state = FootContactState.from_parent_frame_transform( + descriptor=descriptor, transform=stance_phase.transform + ) + for point in foot_state: + point.f = stance_phase.force + output.append(foot_state) + + def append_swing_phase( + start_phase: FootContactPhaseDescriptor, + end_phase: FootContactPhaseDescriptor, + points: int, + ): + full_swing_points = int( + np.ceil((end_phase.activation_time - start_phase.deactivation_time) / dt) + ) + mid_swing_points = min(round(full_swing_points / 2), points) + mid_swing_transforms = transform_interpolator( + start_phase.transform, start_phase.mid_swing_transform, mid_swing_points + ) + for transform in mid_swing_transforms: + foot_state = FootContactState.from_parent_frame_transform( + descriptor=descriptor, transform=transform + ) + for point in foot_state: + point.f = 0.0 + output.append(foot_state) + second_half_points = points - mid_swing_points + if second_half_points == 0: + return + second_half_transforms = transform_interpolator( + start_phase.mid_swing_transform, end_phase.transform, second_half_points + ) + for transform in second_half_transforms: + foot_state = FootContactState.from_parent_frame_transform( + descriptor=descriptor, transform=transform + ) + for point in foot_state: + point.f = end_phase.force + output.append(foot_state) + + if len(phases) == 1 or phases[0].deactivation_time >= end_time: + append_stance_phase(phases[0], number_of_points) + return output + + remaining_points = number_of_points + for i in range(len(phases) - 1): + phase = phases[i] + next_phase = phases[i + 1] + + stance_points = int( + np.ceil((phase.deactivation_time - phase.activation_time) / dt) + ) + stance_points = min(stance_points, remaining_points) + + append_stance_phase(phase, stance_points) + remaining_points -= stance_points + + if remaining_points == 0: + return output + + swing_points = int( + np.ceil((next_phase.activation_time - phase.deactivation_time) / dt) + ) + + swing_points = min(swing_points, remaining_points) + + if swing_points == 0: + continue + + append_swing_phase(phase, next_phase, swing_points) + remaining_points -= swing_points + + if remaining_points == 0: + return output + + last_phase = phases[len(phases) - 1] + append_stance_phase(last_phase, remaining_points) + return output diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 54b1b2a3..d7dfa659 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -1,6 +1,7 @@ import dataclasses from typing import TypeVar +import casadi as cs import liecasadi import numpy as np @@ -133,3 +134,22 @@ class FeetContactPointDescriptors: class FeetContactPoints(OptimizationObject): left: FootContactState = default_composite_field(factory=FootContactState) right: FootContactState = default_composite_field(factory=FootContactState) + + +@dataclasses.dataclass +class FootContactPhaseDescriptor: + transform: liecasadi.SE3 = dataclasses.field(default_factory=liecasadi.SE3) + mid_swing_transform: liecasadi.SE3 = dataclasses.field( + default_factory=liecasadi.SE3 + ) + force: float = dataclasses.field(default=100.0) + activation_time: float = dataclasses.field(default=None) + deactivation_time: float = dataclasses.field(default=None) + + def __post_init__(self) -> None: + self.transform = liecasadi.SE3.from_translation_and_rotation( + cs.DM.zeros(3), liecasadi.SO3.Identity() + ) + self.mid_swing_transform = liecasadi.SE3.from_translation_and_rotation( + cs.DM.zeros(3), liecasadi.SO3.Identity() + ) From 25fa92da6022ad3ee17d7e97e0daa852959766ee Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 11:07:03 +0100 Subject: [PATCH 071/106] Added humanoid state interpolator --- src/hippopt/robot_planning/__init__.py | 13 ++ .../robot_planning/utilities/__init__.py | 7 +- .../robot_planning/utilities/interpolators.py | 172 ++++++++++++++++++ .../robot_planning/variables/contacts.py | 6 + 4 files changed, 197 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index b3557ec9..d64feb94 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -37,13 +37,26 @@ HumanoidStateVisualizer, HumanoidStateVisualizerSettings, ) +from .utilities.interpolators import ( + feet_contact_points_interpolator, + floating_base_system_state_interpolator, + foot_contact_state_interpolator, + free_floating_object_state_interpolator, + humanoid_state_interpolator, + kinematic_tree_state_interpolator, + linear_interpolator, + quaternion_slerp, + transform_interpolator, +) from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor from .variables.contacts import ( ContactPointDescriptor, ContactPointState, ContactPointStateDerivative, + FeetContactPhasesDescriptor, FeetContactPointDescriptors, FeetContactPoints, + FootContactPhaseDescriptor, FootContactState, ) from .variables.floating_base import ( diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index a46e1497..ae47a475 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -1 +1,6 @@ -from . import foot_contact_state_plotter, humanoid_state_visualizer, terrain_descriptor +from . import ( + foot_contact_state_plotter, + humanoid_state_visualizer, + interpolators, + terrain_descriptor, +) diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py index 12abd172..524fe258 100644 --- a/src/hippopt/robot_planning/utilities/interpolators.py +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -5,9 +5,18 @@ from hippopt import StorageType from hippopt.robot_planning.variables.contacts import ( ContactPointDescriptor, + FeetContactPhasesDescriptor, + FeetContactPointDescriptors, + FeetContactPoints, FootContactPhaseDescriptor, FootContactState, ) +from hippopt.robot_planning.variables.floating_base import ( + FloatingBaseSystemState, + FreeFloatingObjectState, + KinematicTreeState, +) +from hippopt.robot_planning.variables.humanoid import HumanoidState def linear_interpolator( @@ -194,3 +203,166 @@ def append_swing_phase( last_phase = phases[len(phases) - 1] append_stance_phase(last_phase, remaining_points) return output + + +def feet_contact_points_interpolator( + phases: FeetContactPhasesDescriptor, + descriptor: FeetContactPointDescriptors, + number_of_points: int, + dt: float, + t0: float = 0.0, +) -> list[FeetContactPoints]: + left_output = foot_contact_state_interpolator( + phases=phases.left, + descriptor=descriptor.left, + number_of_points=number_of_points, + dt=dt, + t0=t0, + ) + right_output = foot_contact_state_interpolator( + phases=phases.right, + descriptor=descriptor.right, + number_of_points=number_of_points, + dt=dt, + t0=t0, + ) + + assert len(left_output) == len(right_output) == number_of_points + + output = [] + for i in range(number_of_points): + output_state = FeetContactPoints() + output_state.left = left_output[i] + output_state.right = right_output[i] + output.append(output_state) + + return output + + +def free_floating_object_state_interpolator( + initial_state: FreeFloatingObjectState, + final_state: FreeFloatingObjectState, + number_of_points: int, +) -> list[FreeFloatingObjectState]: + position_interpolation = linear_interpolator( + initial=initial_state.position, + final=final_state.position, + number_of_points=number_of_points, + ) + quaternion_interpolation = quaternion_slerp( + initial=initial_state.quaternion_xyzw, + final=final_state.quaternion_xyzw, + number_of_points=number_of_points, + ) + assert ( + len(position_interpolation) == len(quaternion_interpolation) == number_of_points + ) + output = [] + for i in range(number_of_points): + output_state = FreeFloatingObjectState() + output_state.position = position_interpolation[i] + output_state.quaternion_xyzw = quaternion_interpolation[i] + output.append(output_state) + return output + + +def kinematic_tree_state_interpolator( + initial_state: KinematicTreeState, + final_state: KinematicTreeState, + number_of_points: int, +) -> list[KinematicTreeState]: + if len(initial_state.positions) != len(final_state.positions): + raise ValueError( + f"Initial state has {len(initial_state.positions)} joints, " + f"but final state has {len(final_state.positions)} joints." + ) + + positions_interpolation = linear_interpolator( + initial=initial_state.positions, + final=final_state.positions, + number_of_points=number_of_points, + ) + output = [] + for i in range(number_of_points): + output_state = KinematicTreeState( + number_of_joints_state=len(initial_state.positions) + ) + output_state.positions = positions_interpolation[i] + output.append(output_state) + return output + + +def floating_base_system_state_interpolator( + initial_state: FloatingBaseSystemState, + final_state: FloatingBaseSystemState, + number_of_points: int, +) -> list[FloatingBaseSystemState]: + base_interpolation = free_floating_object_state_interpolator( + initial_state=initial_state.base, + final_state=final_state.base, + number_of_points=number_of_points, + ) + joints_interpolation = kinematic_tree_state_interpolator( + initial_state=initial_state.joints, + final_state=final_state.joints, + number_of_points=number_of_points, + ) + + assert len(base_interpolation) == len(joints_interpolation) == number_of_points + + output = [] + for i in range(number_of_points): + output_state = FloatingBaseSystemState( + number_of_joints_state=len(initial_state.joints.positions) + ) + output_state.base = base_interpolation[i] + output_state.joints = joints_interpolation[i] + output.append(output_state) + return output + + +def humanoid_state_interpolator( + initial_state: HumanoidState, + final_state: HumanoidState, + contact_phases: FeetContactPhasesDescriptor, + contact_descriptor: FeetContactPointDescriptors, + number_of_points: int, + dt: float, + t0: float = 0.0, +): + contact_points_interpolation = feet_contact_points_interpolator( + phases=contact_phases, + descriptor=contact_descriptor, + number_of_points=number_of_points, + dt=dt, + t0=t0, + ) + kinematics_interpolation = floating_base_system_state_interpolator( + initial_state=initial_state.kinematics, + final_state=final_state.kinematics, + number_of_points=number_of_points, + ) + com_interpolation = linear_interpolator( + initial=initial_state.com, + final=final_state.com, + number_of_points=number_of_points, + ) + + assert ( + len(contact_points_interpolation) + == len(kinematics_interpolation) + == len(com_interpolation) + == number_of_points + ) + + output = [] + for i in range(number_of_points): + output_state = HumanoidState( + contact_point_descriptors=contact_descriptor, + number_of_joints=len(initial_state.kinematics.joints.positions), + ) + output_state.contact_points = contact_points_interpolation[i] + output_state.kinematics = kinematics_interpolation[i] + output_state.com = com_interpolation[i] + output.append(output_state) + return output diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index d7dfa659..051dd7f5 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -153,3 +153,9 @@ def __post_init__(self) -> None: self.mid_swing_transform = liecasadi.SE3.from_translation_and_rotation( cs.DM.zeros(3), liecasadi.SO3.Identity() ) + + +@dataclasses.dataclass +class FeetContactPhasesDescriptor: + left: list[FootContactPhaseDescriptor] = dataclasses.field(default_factory=list) + right: list[FootContactPhaseDescriptor] = dataclasses.field(default_factory=list) From 802ad22a93d36e784bd70d6b177e578cc347b246 Mon Sep 17 00:00:00 2001 From: Stefano Date: Tue, 28 Nov 2023 12:44:13 +0100 Subject: [PATCH 072/106] Catch ffmpeg exceptions when creating the video --- .../utilities/humanoid_state_visualizer.py | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 6bbbe13d..8b6ee468 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -462,7 +462,18 @@ def generate_video_from_frames( f"{self._settings.working_folder}/{file_name_stem}.mp4", video_bitrate="20M", ) - ffmpeg.run(video) + try: + ffmpeg.run(video) + except ffmpeg.Error as e: + self._logger.error( + "ffmpeg failed to generate the video. " + "The following output might contain additional information: " + + str(e) + + " stderr: " + + str(e.stderr) + + " stdout: " + + str(e.stdout) + ) def visualize( self, From 669c4e6981b353d095ddcc10f7f85e13dbcac2b0 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 14:34:12 +0100 Subject: [PATCH 073/106] Robustification of the visualization in case the inputs are not in the correct shape --- .../utilities/humanoid_state_visualizer.py | 14 ++++++++------ 1 file changed, 8 insertions(+), 6 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 8b6ee468..a296262c 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -219,15 +219,15 @@ def _get_force_scaled_rotation(self, point_force: np.ndarray) -> np.ndarray: def _update_clone(self, index: int, state: HumanoidState) -> None: self._viz.set_multibody_system_state( - state.kinematics.base.position, + np.array(state.kinematics.base.position).flatten(), liecasadi.SO3.from_quat(state.kinematics.base.quaternion_xyzw) .as_matrix() .full(), - state.kinematics.joints.positions, + np.array(state.kinematics.joints.positions).flatten(), f"[{index}]robot", ) self._viz.set_primitive_geometry_transform( - state.com, + np.array(state.com).flatten(), np.eye(3), f"[{index}]CoM", ) @@ -236,19 +236,21 @@ def _update_clone(self, index: int, state: HumanoidState) -> None: (state.contact_points.left + state.contact_points.right) ): self._viz.set_primitive_geometry_transform( - point.p, + np.array(point.p).flatten(), np.eye(3), f"[{index}]p_{i}", ) - point_force = point.f * self._settings.contact_force_scaling + point_force = ( + np.array(point.f).flatten() * self._settings.contact_force_scaling + ) # Copied from https://github.com/robotology/idyntree/pull/1087 until it is # available in conda position = point.p + point_force / 2 # the origin is in the cylinder center transform = np.zeros((4, 4)) - transform[0:3, 3] = position + transform[0:3, 3] = np.array(position).flatten() transform[3, 3] = 1 transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) self._viz.viewer[f"[{index}]f_{i}"].set_transform(transform) From 8c2582b220b6387bced2fefc72e41603d4028386 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 14:34:55 +0100 Subject: [PATCH 074/106] Improved the definition of the FootContactPhaseDescriptor --- .../robot_planning/variables/contacts.py | 25 +++++++++---------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 051dd7f5..428cd00d 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -138,21 +138,20 @@ class FeetContactPoints(OptimizationObject): @dataclasses.dataclass class FootContactPhaseDescriptor: - transform: liecasadi.SE3 = dataclasses.field(default_factory=liecasadi.SE3) - mid_swing_transform: liecasadi.SE3 = dataclasses.field( - default_factory=liecasadi.SE3 - ) - force: float = dataclasses.field(default=100.0) - activation_time: float = dataclasses.field(default=None) - deactivation_time: float = dataclasses.field(default=None) + transform: liecasadi.SE3 = dataclasses.field(default=None) + mid_swing_transform: liecasadi.SE3 | None = dataclasses.field(default=None) + force: np.ndarray = dataclasses.field(default=None) + activation_time: float | None = dataclasses.field(default=None) + deactivation_time: float | None = dataclasses.field(default=None) def __post_init__(self) -> None: - self.transform = liecasadi.SE3.from_translation_and_rotation( - cs.DM.zeros(3), liecasadi.SO3.Identity() - ) - self.mid_swing_transform = liecasadi.SE3.from_translation_and_rotation( - cs.DM.zeros(3), liecasadi.SO3.Identity() - ) + if self.transform is None: + self.transform = liecasadi.SE3.from_translation_and_rotation( + cs.DM.zeros(3), liecasadi.SO3.Identity() + ) + if self.force is None: + self.force = np.zeros(3) + self.force[2] = 100 @dataclasses.dataclass From abaebf62d2c0c4f0d829b72ea1a94813eb2ab1b8 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 14:35:06 +0100 Subject: [PATCH 075/106] Interpolators bugfix --- .../robot_planning/utilities/interpolators.py | 74 ++++++++++++++++--- 1 file changed, 65 insertions(+), 9 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py index 524fe258..410e69cd 100644 --- a/src/hippopt/robot_planning/utilities/interpolators.py +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -1,4 +1,5 @@ -import casadi as cs +import math + import liecasadi import numpy as np @@ -24,9 +25,30 @@ def linear_interpolator( ) -> list[StorageType]: assert not isinstance(initial, list) and not isinstance(final, list) - interpolator = cs.interpolant("lerp", "linear", [initial, final], [0.0, 1.0]) - x = np.linspace(start=0.0, stop=1.0, num=number_of_points) - return [interpolator(x_i) for x_i in x] + initial = np.array(initial) + final = np.array(final) + + if len(initial.shape) < 2: + initial = np.expand_dims(initial, axis=1) + + if len(final.shape) < 2: + final = np.expand_dims(final, axis=1) + + if ( + hasattr(initial, "shape") + and hasattr(final, "shape") + and initial.shape != final.shape + ): + raise ValueError( + f"Initial value has shape {initial.shape}, " + f"but final value has shape {final.shape}." + ) + + t = np.linspace(start=0.0, stop=1.0, num=number_of_points) + output = [] + for t_i in t: + output.append((1 - t_i) * initial + t_i * final) + return output def quaternion_slerp( @@ -34,8 +56,27 @@ def quaternion_slerp( ) -> list[StorageType]: assert not isinstance(initial, list) and not isinstance(final, list) - x = np.linspace(start=0.0, stop=1.0, num=number_of_points) - return [liecasadi.Quaternion.slerp_step(initial, final, t) for t in x] + initial = np.array(initial) + final = np.array(final) + + if len(initial.shape) < 2: + initial = np.expand_dims(initial, axis=1) + + if len(final.shape) < 2: + final = np.expand_dims(final, axis=1) + + dot = initial.T @ final + angle = math.acos(dot) + + t = np.linspace(start=0.0, stop=1.0, num=number_of_points) + output = [] + for t_i in t: + output.append( + liecasadi.Quaternion.slerp_step(initial, final, t_i).coeffs() + if abs(angle) > 1e-6 + else initial + ) + return output def transform_interpolator( @@ -47,14 +88,16 @@ def transform_interpolator( number_of_points=number_of_points, ) quaternion_interpolation = quaternion_slerp( - initial=initial.rotation(), - final=final.rotation(), + initial=initial.rotation().as_quat().coeffs(), + final=final.rotation().as_quat().coeffs(), number_of_points=number_of_points, ) output = [] for i in range(number_of_points): output.append( - liecasadi.SE3(quaternion_interpolation[i], linear_interpolation[i]) + liecasadi.SE3.from_position_quaternion( + linear_interpolation[i], quaternion_interpolation[i] + ) ) return output @@ -140,6 +183,19 @@ def append_swing_phase( full_swing_points = int( np.ceil((end_phase.activation_time - start_phase.deactivation_time) / dt) ) + + if start_phase.mid_swing_transform is None: + start_phase.mid_swing_transform = ( + liecasadi.SE3.from_translation_and_rotation( + ( + start_phase.transform.translation() + + end_phase.transform.translation() + ) + / 2, + end_phase.transform.rotation(), + ) + ) + mid_swing_points = min(round(full_swing_points / 2), points) mid_swing_transforms = transform_interpolator( start_phase.transform, start_phase.mid_swing_transform, mid_swing_points From b276b061fc7d77ea2cedd55afb8cab8d8c2fe6bb Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 14:52:26 +0100 Subject: [PATCH 076/106] Using __post_init__ in variables differently Only edit the variables if they are None --- .../robot_planning/variables/contacts.py | 23 ++++-- .../robot_planning/variables/floating_base.py | 76 +++++++++++-------- .../robot_planning/variables/humanoid.py | 11 +-- 3 files changed, 66 insertions(+), 44 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 428cd00d..1ac58e6c 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -29,8 +29,10 @@ class ContactPointDescriptor(OptimizationObject): def __post_init__( self, input_foot_frame: str, input_position_in_foot_frame: np.ndarray ) -> None: - self.foot_frame = input_foot_frame - self.position_in_foot_frame = input_position_in_foot_frame + if input_foot_frame is not None: + self.foot_frame = input_foot_frame + if input_position_in_foot_frame is not None: + self.position_in_foot_frame = input_position_in_foot_frame @staticmethod def rectangular_foot( @@ -76,10 +78,14 @@ class ContactPointState(OptimizationObject): ) def __post_init__(self, input_descriptor: ContactPointDescriptor) -> None: - self.p = np.zeros(3) - self.f = np.zeros(3) + if self.p is None: + self.p = np.zeros(3) - self.descriptor = input_descriptor + if self.f is None: + self.f = np.zeros(3) + + if input_descriptor is not None: + self.descriptor = input_descriptor @dataclasses.dataclass @@ -88,8 +94,11 @@ class ContactPointStateDerivative(OptimizationObject): f_dot: StorageType = default_storage_field(OverridableVariable) def __post_init__(self) -> None: - self.v = np.zeros(3) - self.f_dot = np.zeros(3) + if self.v is None: + self.v = np.zeros(3) + + if self.f_dot is None: + self.f_dot = np.zeros(3) TFootContactState = TypeVar("TFootContactState", bound="FootContactState") diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index f3084e1b..9af82245 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -19,9 +19,11 @@ class FreeFloatingObjectState(OptimizationObject): quaternion_xyzw: StorageType = default_storage_field(OverridableVariable) def __post_init__(self): - self.position = np.zeros(3) - self.quaternion_xyzw = np.zeros(4) - self.quaternion_xyzw[3] = 1.0 + if self.position is None: + self.position = np.zeros(3) + if self.quaternion_xyzw is None: + self.quaternion_xyzw = np.zeros(4) + self.quaternion_xyzw[3] = 1.0 @dataclasses.dataclass @@ -30,8 +32,11 @@ class FreeFloatingObjectStateDerivative(OptimizationObject): quaternion_velocity_xyzw: StorageType = default_storage_field(OverridableVariable) def __post_init__(self): - self.linear_velocity = np.zeros(3) - self.quaternion_velocity_xyzw = np.zeros(4) + if self.linear_velocity is None: + self.linear_velocity = np.zeros(3) + + if self.quaternion_velocity_xyzw is None: + self.quaternion_velocity_xyzw = np.zeros(4) @dataclasses.dataclass @@ -48,7 +53,8 @@ class KinematicTreeState(OptimizationObject): number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=0) def __post_init__(self, number_of_joints_state: int): - self.positions = np.zeros(number_of_joints_state) + if number_of_joints_state is not None: + self.positions = np.zeros(number_of_joints_state) @dataclasses.dataclass @@ -60,7 +66,8 @@ class KinematicTreeStateDerivative(OptimizationObject): ) def __post_init__(self, number_of_joints_derivative: int): - self.velocities = np.zeros(number_of_joints_derivative) + if number_of_joints_derivative is not None: + self.velocities = np.zeros(number_of_joints_derivative) @dataclasses.dataclass @@ -70,26 +77,26 @@ def __post_init__( number_of_joints_derivative: int = None, number_of_joints_state: int = None, ): - if number_of_joints_derivative is None and number_of_joints_state is None: - number_of_joints_state = 0 - number_of_joints_derivative = 0 - - number_of_joints_state = ( - number_of_joints_derivative - if number_of_joints_state is None - else number_of_joints_state - ) - number_of_joints_derivative = ( - number_of_joints_state - if number_of_joints_derivative is None - else number_of_joints_derivative - ) - KinematicTreeState.__post_init__( - self, number_of_joints_state=number_of_joints_state - ) - KinematicTreeStateDerivative.__post_init__( - self, number_of_joints_derivative=number_of_joints_derivative - ) + if ( + number_of_joints_derivative is not None + or number_of_joints_state is not None + ): + number_of_joints_state = ( + number_of_joints_derivative + if number_of_joints_state is None + else number_of_joints_state + ) + number_of_joints_derivative = ( + number_of_joints_state + if number_of_joints_derivative is None + else number_of_joints_derivative + ) + KinematicTreeState.__post_init__( + self, number_of_joints_state=number_of_joints_state + ) + KinematicTreeStateDerivative.__post_init__( + self, number_of_joints_derivative=number_of_joints_derivative + ) @dataclasses.dataclass @@ -104,7 +111,10 @@ class FloatingBaseSystemState(OptimizationObject): number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_joints_state: int): - self.joints = KinematicTreeState(number_of_joints_state=number_of_joints_state) + if number_of_joints_state is not None: + self.joints = KinematicTreeState( + number_of_joints_state=number_of_joints_state + ) @dataclasses.dataclass @@ -121,9 +131,10 @@ class FloatingBaseSystemStateDerivative(OptimizationObject): ) def __post_init__(self, number_of_joints_derivative: int): - self.joints = KinematicTreeStateDerivative( - number_of_joints_derivative=number_of_joints_derivative - ) + if number_of_joints_derivative is not None: + self.joints = KinematicTreeStateDerivative( + number_of_joints_derivative=number_of_joints_derivative + ) TFloatingBaseSystem = TypeVar("TFloatingBaseSystem", bound="FloatingBaseSystem") @@ -141,7 +152,8 @@ class FloatingBaseSystem(OptimizationObject): number_of_joints: dataclasses.InitVar[int] = dataclasses.field(default=None) def __post_init__(self, number_of_joints: int): - self.joints = KinematicTree(number_of_joints_state=number_of_joints) + if number_of_joints is not None: + self.joints = KinematicTree(number_of_joints_state=number_of_joints) def to_floating_base_system_state(self): output = FloatingBaseSystemState() diff --git a/src/hippopt/robot_planning/variables/humanoid.py b/src/hippopt/robot_planning/variables/humanoid.py index 6b732221..47c9fee2 100644 --- a/src/hippopt/robot_planning/variables/humanoid.py +++ b/src/hippopt/robot_planning/variables/humanoid.py @@ -49,8 +49,9 @@ def __post_init__( ContactPointState(input_descriptor=point) for point in contact_point_descriptors.right ] - number_of_joints = number_of_joints if number_of_joints is not None else 0 - self.kinematics = FloatingBaseSystemState( - number_of_joints_state=number_of_joints - ) - self.com = np.zeros(3) + if number_of_joints is not None: + self.kinematics = FloatingBaseSystemState( + number_of_joints_state=number_of_joints + ) + if self.com is None: + self.com = np.zeros(3) From 6fbc310724ae4ac3b7f85c92ae04b9cfed716c46 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 29 Nov 2023 16:43:44 +0100 Subject: [PATCH 077/106] The foot contact state interpolator does not need to start from the first phase But there should be an active phase at t0 --- .../robot_planning/utilities/interpolators.py | 73 +++++++++++++------ 1 file changed, 51 insertions(+), 22 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py index 410e69cd..4cccd15a 100644 --- a/src/hippopt/robot_planning/utilities/interpolators.py +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -1,3 +1,4 @@ +import copy import math import liecasadi @@ -115,34 +116,43 @@ def foot_contact_state_interpolator( end_time = t0 + dt * number_of_points - if phases[0].activation_time is None: + phases_copy = copy.deepcopy(phases) + + if phases_copy[0].activation_time is None: deactivation_time = ( - phases[0].deactivation_time - if phases[0].deactivation_time is not None + phases_copy[0].deactivation_time + if phases_copy[0].deactivation_time is not None else t0 ) - phases[0].activation_time = min(deactivation_time, t0) - dt + phases_copy[0].activation_time = min(deactivation_time, t0) - dt + + if phases_copy[0].activation_time > t0: + raise ValueError( + f"The first phase activation time " + f"({phases_copy[0].activation_time}) is after " + f"the start time ({t0})." + ) - for i, phase in enumerate(phases): + for i, phase in enumerate(phases_copy): if phase.activation_time is None: raise ValueError( f"Phase {i} has no activation time, but is not the first phase." ) - last = len(phases) - 1 - if phases[last].deactivation_time is None: - phases[last].deactivation_time = ( - max(end_time, phases[last].activation_time) + dt + last = len(phases_copy) - 1 + if phases_copy[last].deactivation_time is None: + phases_copy[last].deactivation_time = ( + max(end_time, phases_copy[last].activation_time) + dt ) - if phases[last].deactivation_time < end_time: + if phases_copy[last].deactivation_time < end_time: raise ValueError( f"The Last phase deactivation time " - f"({phases[len(phases) - 1].deactivation_time}) is before " + f"({phases_copy[len(phases_copy) - 1].deactivation_time}) is before " f"the end time ({end_time}, computed from the inputs)." ) - for i, phase in enumerate(phases): + for i, phase in enumerate(phases_copy): if phase.deactivation_time is None: raise ValueError( f"Phase {i} has no deactivation time, but is not the last phase." @@ -154,11 +164,11 @@ def foot_contact_state_interpolator( ) if i < last: - if phase.deactivation_time > phases[i + 1].activation_time: + if phase.deactivation_time > phases_copy[i + 1].activation_time: raise ValueError( f"Phase {i} has a deactivation time ({phase.deactivation_time}) " f"greater than the activation time of the next phase " - f"({phases[i + 1].activation_time})." + f"({phases_copy[i + 1].activation_time})." ) output = [] @@ -205,7 +215,7 @@ def append_swing_phase( descriptor=descriptor, transform=transform ) for point in foot_state: - point.f = 0.0 + point.f = np.zeros((3, 1)) output.append(foot_state) second_half_points = points - mid_swing_points if second_half_points == 0: @@ -218,17 +228,35 @@ def append_swing_phase( descriptor=descriptor, transform=transform ) for point in foot_state: - point.f = end_phase.force + point.f = np.zeros((3, 1)) output.append(foot_state) - if len(phases) == 1 or phases[0].deactivation_time >= end_time: - append_stance_phase(phases[0], number_of_points) + if len(phases_copy) == 1 or phases_copy[0].deactivation_time >= end_time: + append_stance_phase(phases_copy[0], number_of_points) return output + i = 0 + activation_time = phases_copy[0].activation_time + while activation_time < t0: + if phases_copy[i].deactivation_time > t0: + break + i += 1 + activation_time = phases_copy[i].activation_time + + if activation_time > t0: + # One possibility could be to call the interpolator recursively starting + # before in time (with more interpolation points) such that this is not an + # issue, and then keep only the last interpolation_points + raise ValueError( + f"There is no active contacts at t0 ({t0}). " + f"At the moment, starting the interpolation from a swing state" + f" is not supported." + ) + remaining_points = number_of_points - for i in range(len(phases) - 1): - phase = phases[i] - next_phase = phases[i + 1] + while i < len(phases_copy) - 1: + phase = phases_copy[i] + next_phase = phases_copy[i + 1] stance_points = int( np.ceil((phase.deactivation_time - phase.activation_time) / dt) @@ -255,8 +283,9 @@ def append_swing_phase( if remaining_points == 0: return output + i += 1 - last_phase = phases[len(phases) - 1] + last_phase = phases_copy[len(phases_copy) - 1] append_stance_phase(last_phase, remaining_points) return output From f449c34a5cf948423464e34d72b5ed5e189779b7 Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 6 Dec 2023 15:06:47 +0900 Subject: [PATCH 078/106] Allowed interpolating even if the initial time is a swing moment --- .../robot_planning/utilities/interpolators.py | 17 ++++++++++------- 1 file changed, 10 insertions(+), 7 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py index 4cccd15a..62163eee 100644 --- a/src/hippopt/robot_planning/utilities/interpolators.py +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -244,14 +244,17 @@ def append_swing_phase( activation_time = phases_copy[i].activation_time if activation_time > t0: - # One possibility could be to call the interpolator recursively starting - # before in time (with more interpolation points) such that this is not an - # issue, and then keep only the last interpolation_points - raise ValueError( - f"There is no active contacts at t0 ({t0}). " - f"At the moment, starting the interpolation from a swing state" - f" is not supported." + previous_active_phase = phases_copy[i - 1] + new_t0 = previous_active_phase.deactivation_time - dt + advance_points = int(np.ceil((t0 - new_t0) / dt)) + increased_output = foot_contact_state_interpolator( + phases=phases_copy, + descriptor=descriptor, + number_of_points=number_of_points + advance_points, + dt=dt, + t0=new_t0, ) + return increased_output[advance_points:] remaining_points = number_of_points while i < len(phases_copy) - 1: From 128702c253d02024498c96b58b56d27339749933 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Sun, 24 Dec 2023 15:59:18 +0100 Subject: [PATCH 079/106] Fix creation of ground terrain urdf --- .../utilities/humanoid_state_visualizer.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index a296262c..118ade03 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -127,8 +127,8 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: raise ValueError("Settings are not valid.") self._logger = logging.getLogger("[hippopt::HumanoidStateVisualizer]") self._settings = settings - self._create_ground_urdf() - self._create_ground_mesh() + mesh_file = self._create_ground_mesh() + self._create_ground_urdf(mesh_file) self._number_of_clones = self._settings.pre_allocated_clones self._viz = MeshcatVisualizer() self._viz.load_model_from_file( @@ -255,7 +255,7 @@ def _update_clone(self, index: int, state: HumanoidState) -> None: transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) self._viz.viewer[f"[{index}]f_{i}"].set_transform(transform) - def _create_ground_urdf(self) -> None: + def _create_ground_urdf(self, mesh_filename: str) -> None: filename = self._settings.terrain.get_name() + ".urdf" full_filename = os.path.join(self._settings.working_folder, filename) if os.path.exists(full_filename): @@ -267,7 +267,7 @@ def _create_ground_urdf(self) -> None: with open(full_filename, "w") as f: f.write( - """ + f""" @@ -280,7 +280,7 @@ def _create_ground_urdf(self) -> None: - + @@ -289,7 +289,7 @@ def _create_ground_urdf(self) -> None: - + @@ -302,7 +302,7 @@ def _create_ground_urdf(self) -> None: """ ) - def _create_ground_mesh(self) -> None: + def _create_ground_mesh(self) -> str: filename = self._settings.terrain.get_name() + ".stl" full_filename = os.path.join(self._settings.working_folder, filename) if os.path.exists(full_filename): @@ -310,7 +310,7 @@ def _create_ground_mesh(self) -> None: self._logger.info(f"Overwriting {filename}") else: self._logger.info(f"{filename} already exists. Skipping creation.") - return + return full_filename x_step = ( self._settings.ground_x_limits[1] - self._settings.ground_x_limits[0] ) / self._settings.ground_mesh_axis_points @@ -333,6 +333,7 @@ def _create_ground_mesh(self) -> None: height_function_map = self._settings.terrain.height_function().map(x.size) z = -np.array(height_function_map(points).full()).reshape(x.shape) surf2stl.write(full_filename, x, y, z) + return full_filename def _visualize_single_state( self, From 9bfeff8024a2d7e7aff3324336e56afc442c6c41 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 29 Dec 2023 10:56:30 +0100 Subject: [PATCH 080/106] Avoid to use glob when saving video. This is to be compatible with Windows --- .../robot_planning/utilities/humanoid_state_visualizer.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 118ade03..10870c12 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -410,7 +410,7 @@ def _visualize_multiple_states( self._visualize_single_state( visualized_states, save=save, - file_name_stem=f"{folder_name}/{frame_prefix}{i:03}", + file_name_stem=f"{folder_name}/{frame_prefix}{i-number_of_clones:03}", ) end = time.time() elapsed_s = end - start @@ -456,8 +456,7 @@ def generate_video_from_frames( self, file_name_stem: str, frames_folder: str, frames_prefix: str, fps: float ) -> None: frames = ffmpeg.input( - filename=f"{frames_folder}/{frames_prefix}*.png", - pattern_type="glob", + filename=f"{frames_folder}/{frames_prefix}%3d.png", framerate=fps, ) video = ffmpeg.output( From 55bdcd85c07bff0515626e3ce631386bb9ecc8e9 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 29 Dec 2023 11:47:38 +0100 Subject: [PATCH 081/106] Fixed visualization of the last state. It was not visualized --- .../robot_planning/utilities/humanoid_state_visualizer.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 10870c12..541df295 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -396,12 +396,12 @@ def _visualize_multiple_states( frame_prefix = "frame_" - for i in range(number_of_clones, len(states)): + for i in range(number_of_clones, len(states) + 1): initial_index = i - number_of_clones visualized_states = states[initial_index:i] if number_of_clones > 1: self._logger.info( - f"Visualizing states [{i-number_of_clones + 1},{i + 1}]" + f"Visualizing states [{i-number_of_clones + 1},{i}]" f" of {len(states)}." ) else: @@ -414,7 +414,7 @@ def _visualize_multiple_states( ) end = time.time() elapsed_s = end - start - sleep_time = _timestep_s[i] * time_multiplier - elapsed_s + sleep_time = _timestep_s[i - 1] * time_multiplier - elapsed_s time.sleep(max(0.0, sleep_time)) if save: From 72b88fe772a617aa6680d05b247c27f05410f772 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 29 Dec 2023 11:48:02 +0100 Subject: [PATCH 082/106] Moved PlanarTerrain to a dedicated file --- src/hippopt/robot_planning/__init__.py | 3 +- .../robot_planning/utilities/__init__.py | 1 + .../utilities/foot_contact_state_plotter.py | 6 +-- .../utilities/planar_terrain.py | 41 +++++++++++++++++++ .../utilities/terrain_descriptor.py | 38 ----------------- 5 files changed, 46 insertions(+), 43 deletions(-) create mode 100644 src/hippopt/robot_planning/utilities/planar_terrain.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index d64feb94..ad468f2b 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -48,7 +48,8 @@ quaternion_slerp, transform_interpolator, ) -from .utilities.terrain_descriptor import PlanarTerrain, TerrainDescriptor +from .utilities.planar_terrain import PlanarTerrain +from .utilities.terrain_descriptor import TerrainDescriptor from .variables.contacts import ( ContactPointDescriptor, ContactPointState, diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index ae47a475..851ddf94 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -2,5 +2,6 @@ foot_contact_state_plotter, humanoid_state_visualizer, interpolators, + planar_terrain, terrain_descriptor, ) diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py index f1ab3d17..f8b370c7 100644 --- a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -9,10 +9,8 @@ import matplotlib.pyplot as plt import numpy as np -from hippopt.robot_planning.utilities.terrain_descriptor import ( - PlanarTerrain, - TerrainDescriptor, -) +from hippopt.robot_planning.utilities.planar_terrain import PlanarTerrain +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor from hippopt.robot_planning.variables.contacts import ( ContactPointState, FootContactState, diff --git a/src/hippopt/robot_planning/utilities/planar_terrain.py b/src/hippopt/robot_planning/utilities/planar_terrain.py new file mode 100644 index 00000000..6f5d73e5 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/planar_terrain.py @@ -0,0 +1,41 @@ +import casadi as cs + +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + + +class PlanarTerrain(TerrainDescriptor): + def create_height_function(self) -> cs.Function: + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + return cs.Function( + "planar_terrain_height", + [point_position], + [point_position[2]], + [self.get_point_position_name()], + ["point_height"], + self._options, + ) + + def create_normal_direction_function(self) -> cs.Function: + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + return cs.Function( + "planar_terrain_normal", + [point_position], + [cs.MX.eye(3)[:, 2]], + [self.get_point_position_name()], + ["normal_direction"], + self._options, + ) + + def create_orientation_function(self) -> cs.Function: + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + return cs.Function( + "planar_terrain_orientation", + [point_position], + [cs.MX.eye(3)], + [self.get_point_position_name()], + ["plane_rotation"], + self._options, + ) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index fe16905e..595c9eeb 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -68,41 +68,3 @@ def get_point_position_name(self) -> str: def get_name(self) -> str: return self._name if isinstance(self._name, str) else self.__class__.__name__ - - -class PlanarTerrain(TerrainDescriptor): - def create_height_function(self) -> cs.Function: - point_position = cs.MX.sym(self.get_point_position_name(), 3) - - return cs.Function( - "planar_terrain_height", - [point_position], - [point_position[2]], - [self.get_point_position_name()], - ["point_height"], - self._options, - ) - - def create_normal_direction_function(self) -> cs.Function: - point_position = cs.MX.sym(self.get_point_position_name(), 3) - - return cs.Function( - "planar_terrain_normal", - [point_position], - [cs.MX.eye(3)[:, 2]], - [self.get_point_position_name()], - ["normal_direction"], - self._options, - ) - - def create_orientation_function(self) -> cs.Function: - point_position = cs.MX.sym(self.get_point_position_name(), 3) - - return cs.Function( - "planar_terrain_orientation", - [point_position], - [cs.MX.eye(3)], - [self.get_point_position_name()], - ["plane_rotation"], - self._options, - ) From f3beaff26382202d6c41279bef5a58f442da4b1a Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 2 Jan 2024 16:36:54 +0100 Subject: [PATCH 083/106] Added first draft of SmoothTerrain --- src/hippopt/robot_planning/__init__.py | 1 + .../robot_planning/utilities/__init__.py | 1 + .../utilities/smooth_terrain.py | 190 ++++++++++++++++++ .../utilities/terrain_descriptor.py | 28 ++- 4 files changed, 210 insertions(+), 10 deletions(-) create mode 100644 src/hippopt/robot_planning/utilities/smooth_terrain.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index ad468f2b..70fa0255 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -49,6 +49,7 @@ transform_interpolator, ) from .utilities.planar_terrain import PlanarTerrain +from .utilities.smooth_terrain import SmoothTerrain from .utilities.terrain_descriptor import TerrainDescriptor from .variables.contacts import ( ContactPointDescriptor, diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index 851ddf94..831708b2 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -3,5 +3,6 @@ humanoid_state_visualizer, interpolators, planar_terrain, + smooth_terrain, terrain_descriptor, ) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py new file mode 100644 index 00000000..7d6327e4 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -0,0 +1,190 @@ +import dataclasses + +import casadi as cs +import numpy as np + +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + + +@dataclasses.dataclass +class SmoothTerrain(TerrainDescriptor): + """ + Smooth terrain is a terrain with a smooth height function. + The height is defined as follows: + h(x, y) = exp(−g(x, y)^(2s)) π(x, y) + δ. + + Here, g(x, y) ≥ 0 is the equation of a closed curve in the xy-plane, + and π(x, y) is the equation of a plane in the xy-plane, defining the shape of the + terrain when exp(−g(x, y)^(2s)) = 1, i.e. when g(x, y) = 0. + The parameter s ≥ 1 controls the smoothness of the terrain. + δ is a height offset. + + Independently of the value of s, the value of exp(−g(.)^(2s)) is always passing + through 1 at the origin, and through 1/e at the point where g(x,y) = 1. Then, will + tend to zero as g(x, y) → ∞. The parameter s controls how fast exp(−g(.)^(2s)) tends + to zero as g(x, y) grows. By multiplying times the equation of a plane, i.e. + π(x, y), we can control the inclination of the top surface when g(x, y) = 0. + Instead, g(x, y) = 1 controls the shape of the terrain at height 1/e * π(x, y). + + For example, to define a classical step with a square base of dimension 1, + we can use: + g(x, y) = |x|^p + |y|^p + π(x, y) = 1 + where p ≥ 1 is a parameter controlling the sharpness of the edges of the square + at height 1/e. + Here g(x, y) = 0 only at the origin, hence, the top surface is flat at the origin, + and the parameter s determines the sharpness of the top face. + + It is then possible to specify a position offset and a transformation matrix to + move, rotate, and scale the terrain. + In particular, we have that: + [x, y, z]^T = R^T * ([x_i, y_i, z_i]^T - [x_offset, y_offset, z_offset]^T) + where [x_i, y_i, z_i]^T is the position of the point in the inertial frame, R is + the transformation matrix, and [x_offset, y_offset, z_offset]^T is the offset. + + """ + + _shape_function: cs.Function = dataclasses.field(default=None) + _top_surface_function: cs.Function = dataclasses.field(default=None) + _sharpness: float = dataclasses.field(default=None) + _delta: float = dataclasses.field(default=None) + _offset: np.ndarray = dataclasses.field(default=None) + _transformation_matrix: np.ndarray = dataclasses.field(default=None) + + shape_function: dataclasses.InitVar[cs.Function] = dataclasses.field(default=None) + top_surface_function: dataclasses.InitVar[cs.Function] = dataclasses.field( + default=None + ) + sharpness: dataclasses.InitVar[float] = dataclasses.field(default=None) + delta: dataclasses.InitVar[float] = dataclasses.field(default=None) + offset: dataclasses.InitVar[np.ndarray] = dataclasses.field(default=None) + transformation_matrix: dataclasses.InitVar[np.ndarray] = dataclasses.field( + default=None + ) + + def __post_init__( + self, + point_position_name: str = None, + options: dict = None, + name: str = None, + shape_function: cs.Function = None, + top_surface_function: cs.Function = None, + sharpness: float = None, + delta: float = None, + offset: np.ndarray = None, + transformation_matrix: np.ndarray = None, + ): + TerrainDescriptor.__post_init__(self, point_position_name, options, name) + + if self._sharpness is None: + self._sharpness = 10.0 + + if self._delta is None: + self._delta = 0.0 + + if self._offset is None: + self._offset = np.zeros(3) + + if self._transformation_matrix is None: + self._transformation_matrix = np.eye(3) + + point_position = cs.MX.sym(self.get_point_position_name(), 3) + if self._shape_function is None: + self._shape_function = cs.Function( + "smooth_terrain_shape", + [point_position], + [point_position[0] ** 10 + point_position[1] ** 10], + [self.get_point_position_name()], + ["g"], + self._options, + ) + if self._top_surface_function is None: + self._top_surface_function = cs.Function( + "smooth_terrain_top_surface", + [point_position], + [cs.MX.eye(1)], + [self.get_point_position_name()], + ["pi"], + self._options, + ) + + self.set_terrain( + shape_function=shape_function, + top_surface_function=top_surface_function, + sharpness=sharpness, + delta=delta, + offset=offset, + transformation_matrix=transformation_matrix, + ) + + def set_terrain( + self, + shape_function: cs.Function = None, + top_surface_function: cs.Function = None, + sharpness: float = None, + delta: float = None, + offset: np.ndarray = None, + transformation_matrix: np.ndarray = None, + ) -> None: + if shape_function is not None: + self._shape_function = shape_function + + if top_surface_function is not None: + self._top_surface_function = top_surface_function + + if sharpness is not None: + if sharpness < 1: + raise ValueError( + "The sharpness parameter must be greater than or equal to 1." + ) + self._sharpness = sharpness + + if delta is not None: + self._delta = delta + + if offset is not None: + if not isinstance(offset, np.ndarray): + raise TypeError("The offset must be a numpy array.") + if offset.size != 3: + raise ValueError("The offset must be a 3D vector.") + self._offset = offset + + if transformation_matrix is not None: + if not isinstance(transformation_matrix, np.ndarray): + raise TypeError("The transformation matrix must be a numpy matrix.") + if transformation_matrix.shape != (3, 3): + raise ValueError("The transformation matrix must be a 2x2 matrix.") + self._transformation_matrix = transformation_matrix + + self.invalidate_functions() + + def create_height_function(self) -> cs.Function: + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + position_in_terrain_frame = cs.transpose(self._transformation_matrix) @ ( + point_position - self._offset, + ) + + shape = self._shape_function(position_in_terrain_frame) + top_surface = self._top_surface_function(position_in_terrain_frame) + + height = cs.exp(-(shape ** (2 * self._sharpness))) * top_surface + self._delta + + return cs.Function( + "smooth_terrain_height", + [point_position], + [height], + [self.get_point_position_name()], + ["point_height"], + self._options, + ) + + def create_normal_direction_function(self) -> cs.Function: + raise NotImplementedError( + "The normal direction function is not implemented for this terrain." + ) + + def create_orientation_function(self) -> cs.Function: + raise NotImplementedError( + "The orientation function is not implemented for this terrain." + ) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index 595c9eeb..8c5b845b 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -6,12 +6,12 @@ @dataclasses.dataclass class TerrainDescriptor(abc.ABC): - _height_function: cs.Function = dataclasses.field(default=None) - _normal_direction_function: cs.Function = dataclasses.field(default=None) - _orientation_function: cs.Function = dataclasses.field(default=None) + _height_function: cs.Function | None = dataclasses.field(default=None) + _normal_direction_function: cs.Function | None = dataclasses.field(default=None) + _orientation_function: cs.Function | None = dataclasses.field(default=None) _point_position_name: str = dataclasses.field(default="point_position") - _name: str = dataclasses.field(default="terrain") - _options: dict = dataclasses.field(default=None) + _name: str = dataclasses.field(default=None) + _options: dict = dataclasses.field(default_factory=dict) point_position_name: dataclasses.InitVar[str] = dataclasses.field( default="point_position" ) @@ -20,18 +20,21 @@ class TerrainDescriptor(abc.ABC): def __post_init__( self, - point_position_name: str = "point_position", + point_position_name: str = None, options: dict = None, name: str = None, ): self.change_options(point_position_name, options) - self._name = name + if name is not None: + self._name = name def change_options( - self, point_position_name: str = "point_position", options: dict = None, **_ + self, point_position_name: str = None, options: dict = None, **_ ) -> None: - self._options = {} if options is None else options - self._point_position_name = point_position_name + if options is not None: + self._options = options + if point_position_name is not None: + self._point_position_name = point_position_name @abc.abstractmethod def create_height_function(self) -> cs.Function: @@ -68,3 +71,8 @@ def get_point_position_name(self) -> str: def get_name(self) -> str: return self._name if isinstance(self._name, str) else self.__class__.__name__ + + def invalidate_functions(self) -> None: + self._height_function = None + self._normal_direction_function = None + self._orientation_function = None From 0bb4cc102413f35584d350a26f604a164d75b98c Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 2 Jan 2024 17:07:50 +0100 Subject: [PATCH 084/106] Moved terrain visualization to separate file --- src/hippopt/robot_planning/__init__.py | 1 + .../robot_planning/utilities/__init__.py | 1 + .../utilities/humanoid_state_visualizer.py | 173 ++++-------------- .../utilities/terrain_visualizer.py | 167 +++++++++++++++++ 4 files changed, 204 insertions(+), 138 deletions(-) create mode 100644 src/hippopt/robot_planning/utilities/terrain_visualizer.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 70fa0255..8df55b9d 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -51,6 +51,7 @@ from .utilities.planar_terrain import PlanarTerrain from .utilities.smooth_terrain import SmoothTerrain from .utilities.terrain_descriptor import TerrainDescriptor +from .utilities.terrain_visualizer import TerrainVisualizer, TerrainVisualizerSettings from .variables.contacts import ( ContactPointDescriptor, ContactPointState, diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index 831708b2..0a7d53d7 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -5,4 +5,5 @@ planar_terrain, smooth_terrain, terrain_descriptor, + terrain_visualizer, ) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 541df295..b43ed555 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -1,7 +1,6 @@ import copy import dataclasses import logging -import os import pathlib import time @@ -10,8 +9,10 @@ import numpy as np from idyntree.visualize import MeshcatVisualizer -import hippopt.deps.surf2stl as surf2stl -from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor +from hippopt.robot_planning.utilities.terrain_visualizer import ( + TerrainVisualizer, + TerrainVisualizerSettings, +) from hippopt.robot_planning.variables.humanoid import ( FeetContactPointDescriptors, HumanoidState, @@ -19,13 +20,11 @@ @dataclasses.dataclass -class HumanoidStateVisualizerSettings: +class HumanoidStateVisualizerSettings(TerrainVisualizerSettings): robot_model: str = dataclasses.field(default=None) considered_joints: list[str] = dataclasses.field(default=None) contact_points: FeetContactPointDescriptors = dataclasses.field(default=None) robot_color: list[float] = dataclasses.field(default=None) - ground_color: list[float] = dataclasses.field(default=None) - terrain: TerrainDescriptor = dataclasses.field(default=None) com_color: list[float] = dataclasses.field(default=None) com_radius: float = dataclasses.field(default=None) contact_points_color: list[float] = dataclasses.field(default=None) @@ -33,33 +32,42 @@ class HumanoidStateVisualizerSettings: contact_forces_color: list[float] = dataclasses.field(default=None) contact_force_radius: float = dataclasses.field(default=None) contact_force_scaling: float = dataclasses.field(default=None) - working_folder: str = dataclasses.field(default=None) - ground_mesh_axis_points: int = dataclasses.field(default=None) - ground_x_limits: list[float] = dataclasses.field(default=None) - ground_y_limits: list[float] = dataclasses.field(default=None) - overwrite_ground_files: bool = dataclasses.field(default=None) pre_allocated_clones: int = dataclasses.field(default=None) def __post_init__(self): - self.robot_color = [1, 1, 1, 0.25] - self.ground_color = [0.5, 0.5, 0.5, 0.75] - self.com_color = [1, 0, 0, 1] - self.com_radius = 0.02 - self.contact_points_color = [0, 0, 0, 1] - self.contact_forces_color = [1, 0, 0, 1] - self.contact_points_radius = 0.01 - self.contact_force_radius = 0.005 - self.contact_force_scaling = 0.01 - self.ground_x_limits = [-1.5, 1.5] - self.ground_y_limits = [-1.5, 1.5] - self.ground_mesh_axis_points = 200 - self.working_folder = "./" - self.overwrite_ground_files = False - self.pre_allocated_clones = 1 + TerrainVisualizerSettings.__post_init__(self) + if self.robot_color is None: + self.robot_color = [1, 1, 1, 0.25] + + if self.com_color is None: + self.com_color = [1, 0, 0, 1] + + if self.com_radius is None: + self.com_radius = 0.02 + + if self.contact_points_color is None: + self.contact_points_color = [0, 0, 0, 1] + + if self.contact_forces_color is None: + self.contact_forces_color = [1, 0, 0, 1] + + if self.contact_points_radius is None: + self.contact_points_radius = 0.01 + + if self.contact_force_radius is None: + self.contact_force_radius = 0.005 + + if self.contact_force_scaling is None: + self.contact_force_scaling = 0.01 + + if self.pre_allocated_clones is None: + self.pre_allocated_clones = 1 def is_valid(self) -> bool: ok = True logger = logging.getLogger("[hippopt::HumanoidStateVisualizerSettings]") + if not TerrainVisualizerSettings.is_valid(self): + ok = False if self.robot_model is None: logger.error("robot_model is not specified.") ok = False @@ -69,15 +77,9 @@ def is_valid(self) -> bool: if self.contact_points is None: logger.error("contact_points is not specified.") ok = False - if not os.access(self.working_folder, os.W_OK): - logger.error("working_folder is not writable.") - ok = False if len(self.robot_color) != 4: logger.error("robot_color is not specified correctly.") ok = False - if len(self.ground_color) != 4: - logger.error("ground_color is not specified correctly.") - ok = False if len(self.com_color) != 4: logger.error("com_color is not specified correctly.") ok = False @@ -96,22 +98,6 @@ def is_valid(self) -> bool: if self.contact_force_radius <= 0: logger.error("contact_force_radius is not specified correctly.") ok = False - if self.ground_mesh_axis_points <= 0: - logger.error("ground_mesh_axis_points is not specified correctly.") - ok = False - if self.terrain is None: - logger.error("terrain is not specified.") - ok = False - if len(self.ground_x_limits) != 2 or ( - self.ground_x_limits[0] >= self.ground_x_limits[1] - ): - logger.error("ground_x_limits are not specified correctly.") - ok = False - if len(self.ground_y_limits) != 2 or ( - self.ground_y_limits[0] >= self.ground_y_limits[1] - ): - logger.error("ground_y_limits are not specified correctly.") - ok = False if self.contact_force_scaling <= 0: logger.error("contact_force_scaling is not specified correctly.") ok = False @@ -127,18 +113,9 @@ def __init__(self, settings: HumanoidStateVisualizerSettings) -> None: raise ValueError("Settings are not valid.") self._logger = logging.getLogger("[hippopt::HumanoidStateVisualizer]") self._settings = settings - mesh_file = self._create_ground_mesh() - self._create_ground_urdf(mesh_file) self._number_of_clones = self._settings.pre_allocated_clones self._viz = MeshcatVisualizer() - self._viz.load_model_from_file( - model_path=os.path.join( - self._settings.working_folder, - self._settings.terrain.get_name() + ".urdf", - ), - model_name="ground", - color=self._settings.ground_color, - ) + self._terrain_visualizer = TerrainVisualizer(self._settings, self._viz) for i in range(self._number_of_clones): self._allocate_clone(i) if i != 0: @@ -255,86 +232,6 @@ def _update_clone(self, index: int, state: HumanoidState) -> None: transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) self._viz.viewer[f"[{index}]f_{i}"].set_transform(transform) - def _create_ground_urdf(self, mesh_filename: str) -> None: - filename = self._settings.terrain.get_name() + ".urdf" - full_filename = os.path.join(self._settings.working_folder, filename) - if os.path.exists(full_filename): - if self._settings.overwrite_ground_files: - self._logger.info(f"Overwriting {filename}") - else: - self._logger.info(f"{filename} already exists. Skipping creation.") - return - - with open(full_filename, "w") as f: - f.write( - f""" - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - """ - ) - - def _create_ground_mesh(self) -> str: - filename = self._settings.terrain.get_name() + ".stl" - full_filename = os.path.join(self._settings.working_folder, filename) - if os.path.exists(full_filename): - if self._settings.overwrite_ground_files: - self._logger.info(f"Overwriting {filename}") - else: - self._logger.info(f"{filename} already exists. Skipping creation.") - return full_filename - x_step = ( - self._settings.ground_x_limits[1] - self._settings.ground_x_limits[0] - ) / self._settings.ground_mesh_axis_points - y_step = ( - self._settings.ground_y_limits[1] - self._settings.ground_y_limits[0] - ) / self._settings.ground_mesh_axis_points - x = np.arange( - self._settings.ground_x_limits[0], - self._settings.ground_x_limits[1] + x_step, - x_step, - ) - y = np.arange( - self._settings.ground_y_limits[0], - self._settings.ground_y_limits[1] + y_step, - y_step, - ) - x, y = np.meshgrid(x, y) - assert x.shape == y.shape - points = np.array([x.flatten(), y.flatten(), np.zeros(x.size)]) - height_function_map = self._settings.terrain.height_function().map(x.size) - z = -np.array(height_function_map(points).full()).reshape(x.shape) - surf2stl.write(full_filename, x, y, z) - return full_filename - def _visualize_single_state( self, states: list[HumanoidState], diff --git a/src/hippopt/robot_planning/utilities/terrain_visualizer.py b/src/hippopt/robot_planning/utilities/terrain_visualizer.py new file mode 100644 index 00000000..c8685df9 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/terrain_visualizer.py @@ -0,0 +1,167 @@ +import dataclasses +import logging +import os + +import numpy as np +from idyntree.visualize import MeshcatVisualizer + +import hippopt.deps.surf2stl as surf2stl +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + + +@dataclasses.dataclass +class TerrainVisualizerSettings: + terrain: TerrainDescriptor = dataclasses.field(default=None) + terrain_color: list[float] = dataclasses.field(default=None) + working_folder: str = dataclasses.field(default=None) + terrain_mesh_axis_points: int = dataclasses.field(default=None) + terrain_x_limits: list[float] = dataclasses.field(default=None) + terrain_y_limits: list[float] = dataclasses.field(default=None) + overwrite_terrain_files: bool = dataclasses.field(default=None) + + def __post_init__(self): + if self.terrain_color is None: + self.terrain_color = [0.5, 0.5, 0.5, 0.75] + + if self.working_folder is None: + self.working_folder = "./" + + if self.terrain_mesh_axis_points is None: + self.terrain_mesh_axis_points = 200 + + if self.terrain_x_limits is None: + self.terrain_x_limits = [-1.5, 1.5] + + if self.terrain_y_limits is None: + self.terrain_y_limits = [-1.5, 1.5] + + if self.overwrite_terrain_files is None: + self.overwrite_terrain_files = False + + def is_valid(self) -> bool: + ok = True + logger = logging.getLogger("[hippopt::TerrainVisualizerSettings]") + if not os.access(self.working_folder, os.W_OK): + logger.error("working_folder is not writable.") + ok = False + if len(self.terrain_color) != 4: + logger.error("terrain_color is not specified correctly.") + ok = False + if self.terrain_mesh_axis_points <= 0: + logger.error("terrain_mesh_axis_points is not specified correctly.") + ok = False + if self.terrain is None: + logger.error("terrain is not specified.") + ok = False + if len(self.terrain_x_limits) != 2 or ( + self.terrain_x_limits[0] >= self.terrain_x_limits[1] + ): + logger.error("terrain_x_limits are not specified correctly.") + ok = False + if len(self.terrain_y_limits) != 2 or ( + self.terrain_y_limits[0] >= self.terrain_y_limits[1] + ): + logger.error("terrain_y_limits are not specified correctly.") + ok = False + return ok + + +class TerrainVisualizer: + def __init__( + self, settings: TerrainVisualizerSettings, viz: MeshcatVisualizer = None + ) -> None: + if not settings.is_valid(): + raise ValueError("Settings are not valid.") + self._logger = logging.getLogger("[hippopt::TerrainVisualizer]") + self._settings = settings + mesh_file = self._create_ground_mesh() + self._create_ground_urdf(mesh_file) + self._viz = MeshcatVisualizer() if viz is None else viz + self._viz.load_model_from_file( + model_path=os.path.join( + self._settings.working_folder, + self._settings.terrain.get_name() + ".urdf", + ), + model_name="terrain", + color=self._settings.terrain_color, + ) + + def _create_ground_urdf(self, mesh_filename: str) -> None: + filename = self._settings.terrain.get_name() + ".urdf" + full_filename = os.path.join(self._settings.working_folder, filename) + if os.path.exists(full_filename): + if self._settings.overwrite_terrain_files: + self._logger.info(f"Overwriting {filename}") + else: + self._logger.info(f"{filename} already exists. Skipping creation.") + return + + with open(full_filename, "w") as f: + f.write( + f""" + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + """ + ) + + def _create_ground_mesh(self) -> str: + filename = self._settings.terrain.get_name() + ".stl" + full_filename = os.path.join(self._settings.working_folder, filename) + if os.path.exists(full_filename): + if self._settings.overwrite_terrain_files: + self._logger.info(f"Overwriting {filename}") + else: + self._logger.info(f"{filename} already exists. Skipping creation.") + return full_filename + x_step = ( + self._settings.terrain_x_limits[1] - self._settings.terrain_x_limits[0] + ) / self._settings.terrain_mesh_axis_points + y_step = ( + self._settings.terrain_y_limits[1] - self._settings.terrain_y_limits[0] + ) / self._settings.terrain_mesh_axis_points + x = np.arange( + self._settings.terrain_x_limits[0], + self._settings.terrain_x_limits[1] + x_step, + x_step, + ) + y = np.arange( + self._settings.terrain_y_limits[0], + self._settings.terrain_y_limits[1] + y_step, + y_step, + ) + x, y = np.meshgrid(x, y) + assert x.shape == y.shape + points = np.array([x.flatten(), y.flatten(), np.zeros(x.size)]) + height_function_map = self._settings.terrain.height_function().map(x.size) + z = -np.array(height_function_map(points).full()).reshape(x.shape) + surf2stl.write(full_filename, x, y, z) + return full_filename From 30cdbc92345005091d15b6c7b65c5a17e96f8df3 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Tue, 2 Jan 2024 17:43:47 +0100 Subject: [PATCH 085/106] Added first test for SmoothTerrain and debugged --- .../robot_planning/utilities/.gitignore | 2 ++ .../utilities/smooth_terrain.py | 28 +++++++++++++++---- 2 files changed, 24 insertions(+), 6 deletions(-) create mode 100644 src/hippopt/robot_planning/utilities/.gitignore diff --git a/src/hippopt/robot_planning/utilities/.gitignore b/src/hippopt/robot_planning/utilities/.gitignore new file mode 100644 index 00000000..ee4c1191 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/.gitignore @@ -0,0 +1,2 @@ +*.urdf +*.stl diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index 7d6327e4..e2096c3f 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -4,6 +4,10 @@ import numpy as np from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor +from hippopt.robot_planning.utilities.terrain_visualizer import ( + TerrainVisualizer, + TerrainVisualizerSettings, +) @dataclasses.dataclass @@ -26,9 +30,9 @@ class SmoothTerrain(TerrainDescriptor): π(x, y), we can control the inclination of the top surface when g(x, y) = 0. Instead, g(x, y) = 1 controls the shape of the terrain at height 1/e * π(x, y). - For example, to define a classical step with a square base of dimension 1, + For example, to define a classical step with a square base of dimension l, we can use: - g(x, y) = |x|^p + |y|^p + g(x, y) = |2/l * x|^p + |2/l * y|^p π(x, y) = 1 where p ≥ 1 is a parameter controlling the sharpness of the edges of the square at height 1/e. @@ -93,7 +97,7 @@ def __post_init__( self._shape_function = cs.Function( "smooth_terrain_shape", [point_position], - [point_position[0] ** 10 + point_position[1] ** 10], + [(4 * point_position[0]) ** 10 + (4 * point_position[1]) ** 10], [self.get_point_position_name()], ["g"], self._options, @@ -102,7 +106,7 @@ def __post_init__( self._top_surface_function = cs.Function( "smooth_terrain_top_surface", [point_position], - [cs.MX.eye(1)], + [cs.MX(0.2)], [self.get_point_position_name()], ["pi"], self._options, @@ -161,14 +165,18 @@ def set_terrain( def create_height_function(self) -> cs.Function: point_position = cs.MX.sym(self.get_point_position_name(), 3) - position_in_terrain_frame = cs.transpose(self._transformation_matrix) @ ( + position_in_terrain_frame = cs.transpose(self._transformation_matrix) @ cs.MX( point_position - self._offset, ) shape = self._shape_function(position_in_terrain_frame) top_surface = self._top_surface_function(position_in_terrain_frame) - height = cs.exp(-(shape ** (2 * self._sharpness))) * top_surface + self._delta + height = ( + point_position[2] + - cs.exp(-(shape ** (2 * self._sharpness))) * top_surface + - self._delta + ) return cs.Function( "smooth_terrain_height", @@ -188,3 +196,11 @@ def create_orientation_function(self) -> cs.Function: raise NotImplementedError( "The orientation function is not implemented for this terrain." ) + + +if __name__ == "__main__": + viz_settings = TerrainVisualizerSettings() + viz_settings.terrain = SmoothTerrain() + viz_settings.overwrite_terrain_files = True + viz = TerrainVisualizer(viz_settings) + input("Press Enter to exit.") From 00b37441e05f0aed92be61f7b4043b656e645098 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 3 Jan 2024 10:03:26 +0100 Subject: [PATCH 086/106] The height offset in the Smooth Terrain is useless It corresponds to a terrain where g() is always zero and pi a constant value. --- .../utilities/smooth_terrain.py | 20 +++---------------- 1 file changed, 3 insertions(+), 17 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index e2096c3f..5a07e97e 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -15,16 +15,15 @@ class SmoothTerrain(TerrainDescriptor): """ Smooth terrain is a terrain with a smooth height function. The height is defined as follows: - h(x, y) = exp(−g(x, y)^(2s)) π(x, y) + δ. + h(x, y) = exp(−g(x, y)^(2s)) π(x, y). Here, g(x, y) ≥ 0 is the equation of a closed curve in the xy-plane, and π(x, y) is the equation of a plane in the xy-plane, defining the shape of the terrain when exp(−g(x, y)^(2s)) = 1, i.e. when g(x, y) = 0. The parameter s ≥ 1 controls the smoothness of the terrain. - δ is a height offset. Independently of the value of s, the value of exp(−g(.)^(2s)) is always passing - through 1 at the origin, and through 1/e at the point where g(x,y) = 1. Then, will + through 1 when g(x,y) = 0, and through 1/e when g(x,y) = 1. Then, it will tend to zero as g(x, y) → ∞. The parameter s controls how fast exp(−g(.)^(2s)) tends to zero as g(x, y) grows. By multiplying times the equation of a plane, i.e. π(x, y), we can control the inclination of the top surface when g(x, y) = 0. @@ -51,7 +50,6 @@ class SmoothTerrain(TerrainDescriptor): _shape_function: cs.Function = dataclasses.field(default=None) _top_surface_function: cs.Function = dataclasses.field(default=None) _sharpness: float = dataclasses.field(default=None) - _delta: float = dataclasses.field(default=None) _offset: np.ndarray = dataclasses.field(default=None) _transformation_matrix: np.ndarray = dataclasses.field(default=None) @@ -60,7 +58,6 @@ class SmoothTerrain(TerrainDescriptor): default=None ) sharpness: dataclasses.InitVar[float] = dataclasses.field(default=None) - delta: dataclasses.InitVar[float] = dataclasses.field(default=None) offset: dataclasses.InitVar[np.ndarray] = dataclasses.field(default=None) transformation_matrix: dataclasses.InitVar[np.ndarray] = dataclasses.field( default=None @@ -74,7 +71,6 @@ def __post_init__( shape_function: cs.Function = None, top_surface_function: cs.Function = None, sharpness: float = None, - delta: float = None, offset: np.ndarray = None, transformation_matrix: np.ndarray = None, ): @@ -83,9 +79,6 @@ def __post_init__( if self._sharpness is None: self._sharpness = 10.0 - if self._delta is None: - self._delta = 0.0 - if self._offset is None: self._offset = np.zeros(3) @@ -116,7 +109,6 @@ def __post_init__( shape_function=shape_function, top_surface_function=top_surface_function, sharpness=sharpness, - delta=delta, offset=offset, transformation_matrix=transformation_matrix, ) @@ -126,7 +118,6 @@ def set_terrain( shape_function: cs.Function = None, top_surface_function: cs.Function = None, sharpness: float = None, - delta: float = None, offset: np.ndarray = None, transformation_matrix: np.ndarray = None, ) -> None: @@ -143,9 +134,6 @@ def set_terrain( ) self._sharpness = sharpness - if delta is not None: - self._delta = delta - if offset is not None: if not isinstance(offset, np.ndarray): raise TypeError("The offset must be a numpy array.") @@ -173,9 +161,7 @@ def create_height_function(self) -> cs.Function: top_surface = self._top_surface_function(position_in_terrain_frame) height = ( - point_position[2] - - cs.exp(-(shape ** (2 * self._sharpness))) * top_surface - - self._delta + point_position[2] - cs.exp(-(shape ** (2 * self._sharpness))) * top_surface ) return cs.Function( From 8801f87f6a9e41105977ffc0b1c42480b707e4cc Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 3 Jan 2024 13:20:26 +0100 Subject: [PATCH 087/106] Using arrow visualization shipped with iDynTree After the 10.0 version has been released --- .../utilities/humanoid_state_visualizer.py | 38 ++----------------- 1 file changed, 4 insertions(+), 34 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index b43ed555..56963869 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -141,9 +141,8 @@ def _allocate_clone(self, index: int) -> None: shape_name=f"[{index}]p_{i}", color=self._settings.contact_points_color, ) - self._viz.load_cylinder( + self._viz.load_arrow( radius=self._settings.contact_force_radius, - length=1.0, shape_name=f"[{index}]f_{i}", color=self._settings.contact_forces_color, ) @@ -171,29 +170,6 @@ def _skew(x: np.ndarray) -> np.ndarray: ] ) - def _get_force_scaled_rotation(self, point_force: np.ndarray) -> np.ndarray: - force_norm = np.linalg.norm(point_force) - scaling = np.diag([1, 1, force_norm]) - - if force_norm < 1e-6: - return scaling - - force_direction = point_force / force_norm - cos_angle = np.dot(np.array([0, 0, 1]), force_direction) - rotation_axis = self._skew(np.array([0, 0, 1])) @ force_direction - - if np.linalg.norm(rotation_axis) < 1e-6: - return scaling - - skew_symmetric_matrix = self._skew(rotation_axis) - rotation = ( - np.eye(3) - + skew_symmetric_matrix - + np.dot(skew_symmetric_matrix, skew_symmetric_matrix) - * ((1 - cos_angle) / (np.linalg.norm(rotation_axis) ** 2)) - ) - return rotation @ scaling - def _update_clone(self, index: int, state: HumanoidState) -> None: self._viz.set_multibody_system_state( np.array(state.kinematics.base.position).flatten(), @@ -222,15 +198,9 @@ def _update_clone(self, index: int, state: HumanoidState) -> None: np.array(point.f).flatten() * self._settings.contact_force_scaling ) - # Copied from https://github.com/robotology/idyntree/pull/1087 until it is - # available in conda - - position = point.p + point_force / 2 # the origin is in the cylinder center - transform = np.zeros((4, 4)) - transform[0:3, 3] = np.array(position).flatten() - transform[3, 3] = 1 - transform[0:3, 0:3] = self._get_force_scaled_rotation(point_force) - self._viz.viewer[f"[{index}]f_{i}"].set_transform(transform) + self._viz.set_arrow_transform( + origin=point.p, vector=point_force, shape_name=f"[{index}]f_{i}" + ) def _visualize_single_state( self, From 410d5eada4fdafa75fd961480a3f759c1e1fae58 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 3 Jan 2024 14:04:09 +0100 Subject: [PATCH 088/106] Allow visualizing terrain normals --- .../utilities/terrain_visualizer.py | 71 +++++++++++++++++++ 1 file changed, 71 insertions(+) diff --git a/src/hippopt/robot_planning/utilities/terrain_visualizer.py b/src/hippopt/robot_planning/utilities/terrain_visualizer.py index c8685df9..54e33d0b 100644 --- a/src/hippopt/robot_planning/utilities/terrain_visualizer.py +++ b/src/hippopt/robot_planning/utilities/terrain_visualizer.py @@ -18,6 +18,11 @@ class TerrainVisualizerSettings: terrain_x_limits: list[float] = dataclasses.field(default=None) terrain_y_limits: list[float] = dataclasses.field(default=None) overwrite_terrain_files: bool = dataclasses.field(default=None) + draw_terrain_normals: bool = dataclasses.field(default=None) + terrain_normals_color: list[float] = dataclasses.field(default=None) + terrain_normals_radius: float = dataclasses.field(default=None) + terrain_normal_axis_points: int = dataclasses.field(default=None) + terrain_normal_scaling: float = dataclasses.field(default=None) def __post_init__(self): if self.terrain_color is None: @@ -38,6 +43,21 @@ def __post_init__(self): if self.overwrite_terrain_files is None: self.overwrite_terrain_files = False + if self.draw_terrain_normals is None: + self.draw_terrain_normals = False + + if self.terrain_normals_color is None: + self.terrain_normals_color = [1.0, 0.0, 0.0, 1.0] + + if self.terrain_normals_radius is None: + self.terrain_normals_radius = 0.01 + + if self.terrain_normal_axis_points is None: + self.terrain_normal_axis_points = 20 + + if self.terrain_normal_scaling is None: + self.terrain_normal_scaling = 0.1 + def is_valid(self) -> bool: ok = True logger = logging.getLogger("[hippopt::TerrainVisualizerSettings]") @@ -63,6 +83,15 @@ def is_valid(self) -> bool: ): logger.error("terrain_y_limits are not specified correctly.") ok = False + if len(self.terrain_normals_color) != 4: + logger.error("terrain_normals_color is not specified correctly.") + ok = False + if self.terrain_normals_radius <= 0: + logger.error("terrain_normals_radius is not specified correctly.") + ok = False + if self.terrain_normal_axis_points <= 0: + logger.error("terrain_normal_axis_points is not specified correctly.") + ok = False return ok @@ -85,6 +114,8 @@ def __init__( model_name="terrain", color=self._settings.terrain_color, ) + if self._settings.draw_terrain_normals: + self._draw_ground_normals() def _create_ground_urdf(self, mesh_filename: str) -> None: filename = self._settings.terrain.get_name() + ".urdf" @@ -165,3 +196,43 @@ def _create_ground_mesh(self) -> str: z = -np.array(height_function_map(points).full()).reshape(x.shape) surf2stl.write(full_filename, x, y, z) return full_filename + + def _draw_ground_normals(self) -> None: + x_step = ( + self._settings.terrain_x_limits[1] - self._settings.terrain_x_limits[0] + ) / self._settings.terrain_normal_axis_points + y_step = ( + self._settings.terrain_y_limits[1] - self._settings.terrain_y_limits[0] + ) / self._settings.terrain_normal_axis_points + x = np.arange( + self._settings.terrain_x_limits[0], + self._settings.terrain_x_limits[1] + x_step, + x_step, + ) + y = np.arange( + self._settings.terrain_y_limits[0], + self._settings.terrain_y_limits[1] + y_step, + y_step, + ) + x, y = np.meshgrid(x, y) + assert x.shape == y.shape + points = np.array([x.flatten(), y.flatten(), np.zeros(x.size)]) + height_function_map = self._settings.terrain.height_function().map(x.size) + z = -np.array(height_function_map(points).full()).reshape(x.shape) + points = np.array([x.flatten(), y.flatten(), z.flatten()]) + normal_function_map = self._settings.terrain.normal_direction_function().map( + x.size + ) + normals = normal_function_map(points).full() + + for i in range(normals.shape[1]): + self._viz.load_arrow( + radius=self._settings.terrain_normals_radius, + color=self._settings.terrain_normals_color, + shape_name=f"normal_{i}", + ) + self._viz.set_arrow_transform( + origin=points[:, i], + vector=self._settings.terrain_normal_scaling * normals[:, i], + shape_name=f"normal_{i}", + ) From 1671c5b7f8528e2b4141b805eb806fcd083a5d18 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 3 Jan 2024 14:41:51 +0100 Subject: [PATCH 089/106] Allow visualizing terrain frames --- .../utilities/terrain_visualizer.py | 97 ++++++++++++++++--- 1 file changed, 85 insertions(+), 12 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/terrain_visualizer.py b/src/hippopt/robot_planning/utilities/terrain_visualizer.py index 54e33d0b..edbcdc09 100644 --- a/src/hippopt/robot_planning/utilities/terrain_visualizer.py +++ b/src/hippopt/robot_planning/utilities/terrain_visualizer.py @@ -22,7 +22,12 @@ class TerrainVisualizerSettings: terrain_normals_color: list[float] = dataclasses.field(default=None) terrain_normals_radius: float = dataclasses.field(default=None) terrain_normal_axis_points: int = dataclasses.field(default=None) - terrain_normal_scaling: float = dataclasses.field(default=None) + terrain_normal_length: float = dataclasses.field(default=None) + draw_terrain_frames: bool = dataclasses.field(default=None) + terrain_frames_opacity: float = dataclasses.field(default=None) + terrain_frames_axis_length: float = dataclasses.field(default=None) + terrain_frames_axis_radius: float = dataclasses.field(default=None) + terrain_frames_axis_points: int = dataclasses.field(default=None) def __post_init__(self): if self.terrain_color is None: @@ -55,8 +60,23 @@ def __post_init__(self): if self.terrain_normal_axis_points is None: self.terrain_normal_axis_points = 20 - if self.terrain_normal_scaling is None: - self.terrain_normal_scaling = 0.1 + if self.terrain_normal_length is None: + self.terrain_normal_length = 0.1 + + if self.draw_terrain_frames is None: + self.draw_terrain_frames = False + + if self.terrain_frames_opacity is None: + self.terrain_frames_opacity = 0.5 + + if self.terrain_frames_axis_length is None: + self.terrain_frames_axis_length = 0.1 + + if self.terrain_frames_axis_radius is None: + self.terrain_frames_axis_radius = 0.01 + + if self.terrain_frames_axis_points is None: + self.terrain_frames_axis_points = 20 def is_valid(self) -> bool: ok = True @@ -92,6 +112,16 @@ def is_valid(self) -> bool: if self.terrain_normal_axis_points <= 0: logger.error("terrain_normal_axis_points is not specified correctly.") ok = False + if self.terrain_frames_axis_points <= 0: + logger.error("terrain_frames_axis_points is not specified correctly.") + ok = False + if self.terrain_frames_opacity < 0 or self.terrain_frames_opacity > 1: + logger.error("terrain_frames_opacity is not specified correctly.") + ok = False + if self.terrain_frames_axis_radius <= 0: + logger.error("terrain_frames_axis_radius is not specified correctly.") + ok = False + return ok @@ -103,8 +133,8 @@ def __init__( raise ValueError("Settings are not valid.") self._logger = logging.getLogger("[hippopt::TerrainVisualizer]") self._settings = settings - mesh_file = self._create_ground_mesh() - self._create_ground_urdf(mesh_file) + mesh_file = self._create_terrain_mesh() + self._create_terrain_urdf(mesh_file) self._viz = MeshcatVisualizer() if viz is None else viz self._viz.load_model_from_file( model_path=os.path.join( @@ -115,9 +145,11 @@ def __init__( color=self._settings.terrain_color, ) if self._settings.draw_terrain_normals: - self._draw_ground_normals() + self._draw_terrain_normals() + if self._settings.draw_terrain_frames: + self._draw_terrain_frames() - def _create_ground_urdf(self, mesh_filename: str) -> None: + def _create_terrain_urdf(self, mesh_filename: str) -> None: filename = self._settings.terrain.get_name() + ".urdf" full_filename = os.path.join(self._settings.working_folder, filename) if os.path.exists(full_filename): @@ -130,7 +162,7 @@ def _create_ground_urdf(self, mesh_filename: str) -> None: with open(full_filename, "w") as f: f.write( f""" - + @@ -144,7 +176,7 @@ def _create_ground_urdf(self, mesh_filename: str) -> None: - + @@ -164,7 +196,7 @@ def _create_ground_urdf(self, mesh_filename: str) -> None: """ ) - def _create_ground_mesh(self) -> str: + def _create_terrain_mesh(self) -> str: filename = self._settings.terrain.get_name() + ".stl" full_filename = os.path.join(self._settings.working_folder, filename) if os.path.exists(full_filename): @@ -197,7 +229,7 @@ def _create_ground_mesh(self) -> str: surf2stl.write(full_filename, x, y, z) return full_filename - def _draw_ground_normals(self) -> None: + def _draw_terrain_normals(self) -> None: x_step = ( self._settings.terrain_x_limits[1] - self._settings.terrain_x_limits[0] ) / self._settings.terrain_normal_axis_points @@ -233,6 +265,47 @@ def _draw_ground_normals(self) -> None: ) self._viz.set_arrow_transform( origin=points[:, i], - vector=self._settings.terrain_normal_scaling * normals[:, i], + vector=self._settings.terrain_normal_length * normals[:, i], shape_name=f"normal_{i}", ) + + def _draw_terrain_frames(self) -> None: + x_step = ( + self._settings.terrain_x_limits[1] - self._settings.terrain_x_limits[0] + ) / self._settings.terrain_frames_axis_points + y_step = ( + self._settings.terrain_y_limits[1] - self._settings.terrain_y_limits[0] + ) / self._settings.terrain_frames_axis_points + x = np.arange( + self._settings.terrain_x_limits[0], + self._settings.terrain_x_limits[1] + x_step, + x_step, + ) + y = np.arange( + self._settings.terrain_y_limits[0], + self._settings.terrain_y_limits[1] + y_step, + y_step, + ) + x, y = np.meshgrid(x, y) + assert x.shape == y.shape + points = np.array([x.flatten(), y.flatten(), np.zeros(x.size)]) + height_function_map = self._settings.terrain.height_function().map(x.size) + z = -np.array(height_function_map(points).full()).reshape(x.shape) + points = np.array([x.flatten(), y.flatten(), z.flatten()]) + + for i in range(x.size): + frame = self._settings.terrain.orientation_function()(points[:, i]).full() + for el in range(3): + color = [0.0, 0.0, 0.0, 0.0] + color[el] = 1.0 + color[3] = self._settings.terrain_frames_opacity + self._viz.load_arrow( + radius=self._settings.terrain_frames_axis_radius, + color=color, + shape_name=f"frame_{i}_{el}", + ) + self._viz.set_arrow_transform( + origin=points[:, i], + vector=self._settings.terrain_frames_axis_length * frame[:, el], + shape_name=f"frame_{i}_{el}", + ) From 52f84a830332cda80662768e6d16d4159c226ed3 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 3 Jan 2024 14:42:53 +0100 Subject: [PATCH 090/106] Added computation of SmoothTerrain normals and frames --- .../utilities/smooth_terrain.py | 101 +++++++++++++++--- 1 file changed, 85 insertions(+), 16 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index 5a07e97e..09fa9a68 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -85,22 +85,22 @@ def __post_init__( if self._transformation_matrix is None: self._transformation_matrix = np.eye(3) - point_position = cs.MX.sym(self.get_point_position_name(), 3) + point_position_xy = cs.MX.sym(self.get_point_position_name() + "_xy", 2) if self._shape_function is None: self._shape_function = cs.Function( "smooth_terrain_shape", - [point_position], - [(4 * point_position[0]) ** 10 + (4 * point_position[1]) ** 10], - [self.get_point_position_name()], + [point_position_xy], + [(2 * point_position_xy[0]) ** 10 + (2 * point_position_xy[1]) ** 10], + [point_position_xy.name()], ["g"], self._options, ) if self._top_surface_function is None: self._top_surface_function = cs.Function( "smooth_terrain_top_surface", - [point_position], - [cs.MX(0.2)], - [self.get_point_position_name()], + [point_position_xy], + [cs.MX(0.5)], + [point_position_xy.name()], ["pi"], self._options, ) @@ -121,10 +121,35 @@ def set_terrain( offset: np.ndarray = None, transformation_matrix: np.ndarray = None, ) -> None: - if shape_function is not None: + if isinstance(shape_function, cs.Function): + if shape_function.n_in() != 1: + raise ValueError( + "The shape function must have exactly one input argument." + ) + if shape_function.n_out() != 1: + raise ValueError( + "The shape function must have exactly one output argument." + ) + if shape_function.numel_in() != 2: + raise ValueError( + "The input argument of the shape function must be a 2D vector." + ) self._shape_function = shape_function - if top_surface_function is not None: + if isinstance(top_surface_function, cs.Function): + if top_surface_function.n_in() != 1: + raise ValueError( + "The top surface function must have exactly one input argument." + ) + if top_surface_function.n_out() != 1: + raise ValueError( + "The top surface function must have exactly one output argument." + ) + if top_surface_function.numel_in() != 2: + raise ValueError( + "The input argument of the top surface function" + " must be a 2D vector." + ) self._top_surface_function = top_surface_function if sharpness is not None: @@ -146,6 +171,14 @@ def set_terrain( raise TypeError("The transformation matrix must be a numpy matrix.") if transformation_matrix.shape != (3, 3): raise ValueError("The transformation matrix must be a 2x2 matrix.") + if ( + np.abs(np.linalg.det(transformation_matrix)) < 1e-6 + or (np.linalg.norm(transformation_matrix, axis=0) < 1e-6).any() + ): + raise ValueError( + "The transformation matrix must be invertible and have a non-zero" + " norm for each column." + ) self._transformation_matrix = transformation_matrix self.invalidate_functions() @@ -153,12 +186,12 @@ def set_terrain( def create_height_function(self) -> cs.Function: point_position = cs.MX.sym(self.get_point_position_name(), 3) - position_in_terrain_frame = cs.transpose(self._transformation_matrix) @ cs.MX( + position_in_terrain_frame = np.linalg.inv(self._transformation_matrix) @ cs.MX( point_position - self._offset, ) - shape = self._shape_function(position_in_terrain_frame) - top_surface = self._top_surface_function(position_in_terrain_frame) + shape = self._shape_function(position_in_terrain_frame[:2]) + top_surface = self._top_surface_function(position_in_terrain_frame[:2]) height = ( point_position[2] - cs.exp(-(shape ** (2 * self._sharpness))) * top_surface @@ -174,13 +207,48 @@ def create_height_function(self) -> cs.Function: ) def create_normal_direction_function(self) -> cs.Function: - raise NotImplementedError( - "The normal direction function is not implemented for this terrain." + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + # The normal direction if the gradient of the implicit function h(x, y, z) = 0 + height_gradient = cs.gradient( + self.height_function()(point_position), point_position + ) + normal_direction = height_gradient / cs.norm_2(height_gradient) + + return cs.Function( + "smooth_terrain_normal", + [point_position], + [normal_direction], + [self.get_point_position_name()], + ["normal_direction"], + self._options, ) def create_orientation_function(self) -> cs.Function: - raise NotImplementedError( - "The orientation function is not implemented for this terrain." + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + normal_direction = self.normal_direction_function()(point_position) + transformation_matrix_inverse = np.linalg.inv(self._transformation_matrix) + normal_direction_in_plane = transformation_matrix_inverse @ normal_direction + + # Here we assume that since the shape and top surface functions do not depend on + # z, the normal direction should always keep some z-component, i.e. the terrain + # normal should not be perfectly parallel to the x-axis in the original + # coordinates. This is to avoid using an if to check whether the normal is + # parallel to the x-axis, which would make the function not easily + # differentiable. + y_direction_plane = cs.cross(normal_direction_in_plane, cs.DM([1, 0, 0])) + y_direction = self._transformation_matrix @ y_direction_plane + y_direction = y_direction / cs.norm_2(y_direction) + x_direction = cs.cross(y_direction, normal_direction) + + return cs.Function( + "smooth_terrain_orientation", + [point_position], + [cs.horzcat(x_direction, y_direction, normal_direction)], + [self.get_point_position_name()], + ["plane_rotation"], + self._options, ) @@ -188,5 +256,6 @@ def create_orientation_function(self) -> cs.Function: viz_settings = TerrainVisualizerSettings() viz_settings.terrain = SmoothTerrain() viz_settings.overwrite_terrain_files = True + viz_settings.draw_terrain_frames = True viz = TerrainVisualizer(viz_settings) input("Press Enter to exit.") From decc7192c76240473b158854df1edd672946fd5b Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Wed, 3 Jan 2024 16:13:13 +0100 Subject: [PATCH 091/106] Improved the effect of the offset and transformation matrix applied to the SmoothTerrain --- .../utilities/smooth_terrain.py | 38 ++++++++++++++++--- 1 file changed, 32 insertions(+), 6 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index 09fa9a68..3c9ee2c9 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -44,6 +44,9 @@ class SmoothTerrain(TerrainDescriptor): [x, y, z]^T = R^T * ([x_i, y_i, z_i]^T - [x_offset, y_offset, z_offset]^T) where [x_i, y_i, z_i]^T is the position of the point in the inertial frame, R is the transformation matrix, and [x_offset, y_offset, z_offset]^T is the offset. + When applying a transformation matrix, it is also possible to rotate the terrain. + Because of the choice of the projection method, the modified z axis needs to be + parallel to the original one (i.e. no rotations around x or y are allowed). """ @@ -170,7 +173,7 @@ def set_terrain( if not isinstance(transformation_matrix, np.ndarray): raise TypeError("The transformation matrix must be a numpy matrix.") if transformation_matrix.shape != (3, 3): - raise ValueError("The transformation matrix must be a 2x2 matrix.") + raise ValueError("The transformation matrix must be a 3x3 matrix.") if ( np.abs(np.linalg.det(transformation_matrix)) < 1e-6 or (np.linalg.norm(transformation_matrix, axis=0) < 1e-6).any() @@ -179,6 +182,13 @@ def set_terrain( "The transformation matrix must be invertible and have a non-zero" " norm for each column." ) + if np.abs(np.dot(transformation_matrix[:, 2], [0, 0, 1])) < ( + 1 - 1e-6 + ) * np.linalg.norm(transformation_matrix[:, 2]): + raise ValueError( + "The transformation matrix should not change" + " the z axis orientation." + ) self._transformation_matrix = transformation_matrix self.invalidate_functions() @@ -193,10 +203,15 @@ def create_height_function(self) -> cs.Function: shape = self._shape_function(position_in_terrain_frame[:2]) top_surface = self._top_surface_function(position_in_terrain_frame[:2]) - height = ( - point_position[2] - cs.exp(-(shape ** (2 * self._sharpness))) * top_surface + z_terrain = cs.exp(-(shape ** (2 * self._sharpness))) * top_surface + terrain_position = ( + self._transformation_matrix + @ cs.vertcat(position_in_terrain_frame[:2], z_terrain) + + self._offset ) + height = point_position[2] - terrain_position[2] + return cs.Function( "smooth_terrain_height", [point_position], @@ -209,7 +224,7 @@ def create_height_function(self) -> cs.Function: def create_normal_direction_function(self) -> cs.Function: point_position = cs.MX.sym(self.get_point_position_name(), 3) - # The normal direction if the gradient of the implicit function h(x, y, z) = 0 + # The normal direction is the gradient of the implicit function h(x, y, z) = 0 height_gradient = cs.gradient( self.height_function()(point_position), point_position ) @@ -239,8 +254,10 @@ def create_orientation_function(self) -> cs.Function: # differentiable. y_direction_plane = cs.cross(normal_direction_in_plane, cs.DM([1, 0, 0])) y_direction = self._transformation_matrix @ y_direction_plane - y_direction = y_direction / cs.norm_2(y_direction) x_direction = cs.cross(y_direction, normal_direction) + x_direction = x_direction / cs.norm_2(x_direction) + # Make sure the y direction is orthogonal even after the transformation + y_direction = cs.cross(normal_direction, x_direction) return cs.Function( "smooth_terrain_orientation", @@ -254,7 +271,16 @@ def create_orientation_function(self) -> cs.Function: if __name__ == "__main__": viz_settings = TerrainVisualizerSettings() - viz_settings.terrain = SmoothTerrain() + rotation_x = np.array( + [ + [np.cos(np.pi / 4), -np.sin(np.pi / 4), 0], + [np.sin(np.pi / 4), np.cos(np.pi / 4), 0], + [0, 0, 1], + ] + ) + viz_settings.terrain = SmoothTerrain( + transformation_matrix=rotation_x, offset=np.array([0, 0, 0.2]) + ) viz_settings.overwrite_terrain_files = True viz_settings.draw_terrain_frames = True viz = TerrainVisualizer(viz_settings) From d5ad36137ec2f128d5f0f90fea5a03f6d51cf2f5 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 11:15:51 +0100 Subject: [PATCH 092/106] Moved computation of normal and terrain orientation directly in terrain descriptor --- .../utilities/smooth_terrain.py | 47 ------------------- .../utilities/terrain_descriptor.py | 37 +++++++++++++-- .../utilities/terrain_visualizer.py | 1 + 3 files changed, 34 insertions(+), 51 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index 3c9ee2c9..db5d58a2 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -221,53 +221,6 @@ def create_height_function(self) -> cs.Function: self._options, ) - def create_normal_direction_function(self) -> cs.Function: - point_position = cs.MX.sym(self.get_point_position_name(), 3) - - # The normal direction is the gradient of the implicit function h(x, y, z) = 0 - height_gradient = cs.gradient( - self.height_function()(point_position), point_position - ) - normal_direction = height_gradient / cs.norm_2(height_gradient) - - return cs.Function( - "smooth_terrain_normal", - [point_position], - [normal_direction], - [self.get_point_position_name()], - ["normal_direction"], - self._options, - ) - - def create_orientation_function(self) -> cs.Function: - point_position = cs.MX.sym(self.get_point_position_name(), 3) - - normal_direction = self.normal_direction_function()(point_position) - transformation_matrix_inverse = np.linalg.inv(self._transformation_matrix) - normal_direction_in_plane = transformation_matrix_inverse @ normal_direction - - # Here we assume that since the shape and top surface functions do not depend on - # z, the normal direction should always keep some z-component, i.e. the terrain - # normal should not be perfectly parallel to the x-axis in the original - # coordinates. This is to avoid using an if to check whether the normal is - # parallel to the x-axis, which would make the function not easily - # differentiable. - y_direction_plane = cs.cross(normal_direction_in_plane, cs.DM([1, 0, 0])) - y_direction = self._transformation_matrix @ y_direction_plane - x_direction = cs.cross(y_direction, normal_direction) - x_direction = x_direction / cs.norm_2(x_direction) - # Make sure the y direction is orthogonal even after the transformation - y_direction = cs.cross(normal_direction, x_direction) - - return cs.Function( - "smooth_terrain_orientation", - [point_position], - [cs.horzcat(x_direction, y_direction, normal_direction)], - [self.get_point_position_name()], - ["plane_rotation"], - self._options, - ) - if __name__ == "__main__": viz_settings = TerrainVisualizerSettings() diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index 8c5b845b..fbfd3065 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -40,13 +40,42 @@ def change_options( def create_height_function(self) -> cs.Function: pass - @abc.abstractmethod def create_normal_direction_function(self) -> cs.Function: - pass + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + # The normal direction is the gradient of the implicit function h(x, y, z) = 0 + height_gradient = cs.gradient( + self.height_function()(point_position), point_position + ) + normal_direction = height_gradient / cs.norm_2(height_gradient) + + return cs.Function( + "terrain_normal", + [point_position], + [normal_direction], + [self.get_point_position_name()], + ["normal_direction"], + self._options, + ) - @abc.abstractmethod def create_orientation_function(self) -> cs.Function: - pass + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + normal_direction = self.normal_direction_function()(point_position) + y_direction = cs.cross(normal_direction, cs.DM([1, 0, 0])) + x_direction = cs.cross(y_direction, normal_direction) + x_direction = x_direction / cs.norm_2(x_direction) + # Make sure the y direction is orthogonal even after the transformation + y_direction = cs.cross(normal_direction, x_direction) + + return cs.Function( + "terrain_orientation", + [point_position], + [cs.horzcat(x_direction, y_direction, normal_direction)], + [self.get_point_position_name()], + ["plane_rotation"], + self._options, + ) def height_function(self) -> cs.Function: if not isinstance(self._height_function, cs.Function): diff --git a/src/hippopt/robot_planning/utilities/terrain_visualizer.py b/src/hippopt/robot_planning/utilities/terrain_visualizer.py index edbcdc09..bf5ce0f9 100644 --- a/src/hippopt/robot_planning/utilities/terrain_visualizer.py +++ b/src/hippopt/robot_planning/utilities/terrain_visualizer.py @@ -295,6 +295,7 @@ def _draw_terrain_frames(self) -> None: for i in range(x.size): frame = self._settings.terrain.orientation_function()(points[:, i]).full() + assert np.abs(np.linalg.det(frame) - 1.0) < 1e-6 for el in range(3): color = [0.0, 0.0, 0.0, 0.0] color[el] = 1.0 From b25e8a6a81cfe2dbe4070c9156bbe69c84e9c2a2 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 11:51:16 +0100 Subject: [PATCH 093/106] Added possibility to sum smooth terrains --- src/hippopt/robot_planning/__init__.py | 1 + .../robot_planning/utilities/__init__.py | 1 + .../utilities/smooth_terrain.py | 11 +++- .../robot_planning/utilities/terrain_sum.py | 55 +++++++++++++++++++ 4 files changed, 66 insertions(+), 2 deletions(-) create mode 100644 src/hippopt/robot_planning/utilities/terrain_sum.py diff --git a/src/hippopt/robot_planning/__init__.py b/src/hippopt/robot_planning/__init__.py index 8df55b9d..a6cbdf54 100644 --- a/src/hippopt/robot_planning/__init__.py +++ b/src/hippopt/robot_planning/__init__.py @@ -51,6 +51,7 @@ from .utilities.planar_terrain import PlanarTerrain from .utilities.smooth_terrain import SmoothTerrain from .utilities.terrain_descriptor import TerrainDescriptor +from .utilities.terrain_sum import TerrainSum from .utilities.terrain_visualizer import TerrainVisualizer, TerrainVisualizerSettings from .variables.contacts import ( ContactPointDescriptor, diff --git a/src/hippopt/robot_planning/utilities/__init__.py b/src/hippopt/robot_planning/utilities/__init__.py index 0a7d53d7..49c6432d 100644 --- a/src/hippopt/robot_planning/utilities/__init__.py +++ b/src/hippopt/robot_planning/utilities/__init__.py @@ -5,5 +5,6 @@ planar_terrain, smooth_terrain, terrain_descriptor, + terrain_sum, terrain_visualizer, ) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index db5d58a2..302171be 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -4,6 +4,7 @@ import numpy as np from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor +from hippopt.robot_planning.utilities.terrain_sum import TerrainSum from hippopt.robot_planning.utilities.terrain_visualizer import ( TerrainVisualizer, TerrainVisualizerSettings, @@ -221,6 +222,12 @@ def create_height_function(self) -> cs.Function: self._options, ) + def __add__(self, other: TerrainDescriptor) -> TerrainSum: + return TerrainSum.add(self, other) + + def __radd__(self, other: TerrainDescriptor) -> TerrainSum: + return TerrainSum.add(other, self) + if __name__ == "__main__": viz_settings = TerrainVisualizerSettings() @@ -232,8 +239,8 @@ def create_height_function(self) -> cs.Function: ] ) viz_settings.terrain = SmoothTerrain( - transformation_matrix=rotation_x, offset=np.array([0, 0, 0.2]) - ) + transformation_matrix=rotation_x, offset=np.array([-0.5, -0.5, 0.0]) + ) + SmoothTerrain(offset=np.array([0.8, 0.5, 0.0])) viz_settings.overwrite_terrain_files = True viz_settings.draw_terrain_frames = True viz = TerrainVisualizer(viz_settings) diff --git a/src/hippopt/robot_planning/utilities/terrain_sum.py b/src/hippopt/robot_planning/utilities/terrain_sum.py new file mode 100644 index 00000000..bf813663 --- /dev/null +++ b/src/hippopt/robot_planning/utilities/terrain_sum.py @@ -0,0 +1,55 @@ +import dataclasses +from typing import TypeVar + +import casadi as cs + +from hippopt.robot_planning.utilities.terrain_descriptor import TerrainDescriptor + +TTerrainSum = TypeVar("TTerrainSum", bound="TerrainSum") + + +class TerrainSum(TerrainDescriptor): + _lhs: TerrainDescriptor = dataclasses.field(default=None) + _rhs: TerrainDescriptor = dataclasses.field(default=None) + + def set_terms(self, lhs: TerrainDescriptor, rhs: TerrainDescriptor): + self._lhs = lhs + self._rhs = rhs + + def create_height_function(self) -> cs.Function: + assert ( + self._lhs is not None and self._rhs is not None + ), "Both lhs and rhs must be provided" + + point_position = cs.MX.sym(self.get_point_position_name(), 3) + + # We need to subtract the point height because otherwise is counted twice + return cs.Function( + "terrain_sum_height", + [point_position], + [ + self._lhs.height_function()(point_position) + + self._rhs.height_function()(point_position) + - point_position[2] + ], + [self.get_point_position_name()], + ["point_height"], + self._options, + ) + + @classmethod + def add(cls, lhs: TerrainDescriptor, rhs: TerrainDescriptor) -> TTerrainSum: + output = cls( + point_position_name=lhs.get_point_position_name(), + options=lhs._options, + name=f"{lhs.get_name()}+{rhs.get_name()}", + ) + output.set_terms(lhs, rhs) + + return output + + def __add__(self, other: TTerrainSum) -> TTerrainSum: + return TerrainSum.add(self, other) + + def __radd__(self, other: TTerrainSum) -> TTerrainSum: + return TerrainSum.add(other, self) From e56808f3fd9f0369a0351c3ccf36660503d290f2 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 12:55:41 +0100 Subject: [PATCH 094/106] Removed leftover in humanoid_state_visualizer.py --- .../utilities/humanoid_state_visualizer.py | 10 ---------- 1 file changed, 10 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index 56963869..d2480fc7 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -160,16 +160,6 @@ def _set_clone_visibility(self, index: int, visible: bool) -> None: key="visible", value=visible ) - @staticmethod - def _skew(x: np.ndarray) -> np.ndarray: - return np.array( - [ - [0, -x[2], x[1]], - [x[2], 0, -x[0]], - [-x[1], x[0], 0], - ] - ) - def _update_clone(self, index: int, state: HumanoidState) -> None: self._viz.set_multibody_system_state( np.array(state.kinematics.base.position).flatten(), From 91b6210eec78841bac1be69cd34f22037f37b464 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 13:13:26 +0100 Subject: [PATCH 095/106] Added shortcut functions to create steps, cylinders and planes --- .../utilities/smooth_terrain.py | 251 +++++++++++++++++- 1 file changed, 239 insertions(+), 12 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index 302171be..a400429f 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -1,4 +1,5 @@ import dataclasses +from typing import TypeVar import casadi as cs import numpy as np @@ -10,6 +11,8 @@ TerrainVisualizerSettings, ) +TSmoothTerrain = TypeVar("TSmoothTerrain", bound="SmoothTerrain") + @dataclasses.dataclass class SmoothTerrain(TerrainDescriptor): @@ -81,7 +84,7 @@ def __post_init__( TerrainDescriptor.__post_init__(self, point_position_name, options, name) if self._sharpness is None: - self._sharpness = 10.0 + self._sharpness = 1.0 if self._offset is None: self._offset = np.zeros(3) @@ -94,7 +97,7 @@ def __post_init__( self._shape_function = cs.Function( "smooth_terrain_shape", [point_position_xy], - [(2 * point_position_xy[0]) ** 10 + (2 * point_position_xy[1]) ** 10], + [0], [point_position_xy.name()], ["g"], self._options, @@ -103,7 +106,7 @@ def __post_init__( self._top_surface_function = cs.Function( "smooth_terrain_top_surface", [point_position_xy], - [cs.MX(0.5)], + [0], [point_position_xy.name()], ["pi"], self._options, @@ -228,19 +231,243 @@ def __add__(self, other: TerrainDescriptor) -> TerrainSum: def __radd__(self, other: TerrainDescriptor) -> TerrainSum: return TerrainSum.add(other, self) + @staticmethod + def _top_expression_from_normal( + height: float, point_position_xy: cs.MX, top_normal_direction: np.ndarray + ): + top_expression = cs.MX(height) + if top_normal_direction is not None: + if not isinstance(top_normal_direction, np.ndarray): + raise TypeError("The top normal direction must be a numpy array.") + if top_normal_direction.size != 3: + raise ValueError("The top normal direction must be a 3D vector.") + + norm = np.linalg.norm(top_normal_direction) + if norm < 1e-6: + raise ValueError("The top normal direction must be non-zero.") + + top_normal_direction = top_normal_direction / norm + if np.abs(top_normal_direction[2]) < 1e-6: + raise ValueError( + "The top normal direction must not be parallel to the xy-plane." + ) + top_expression = cs.MX( + -top_normal_direction[0] + / top_normal_direction[2] + * point_position_xy[0] + - top_normal_direction[1] + / top_normal_direction[2] + * point_position_xy[1] + + height + ) + return top_expression + + @classmethod + def step( + cls, + length: float, + width: float, + height: float, + edge_sharpness: float = 5, + side_sharpness: float = 10, + position: np.ndarray = None, + orientation: float = 0.0, + top_normal_direction: np.ndarray = None, + point_position_name: str = None, + options: dict = None, + name: str = None, + ) -> TSmoothTerrain: + if edge_sharpness < 1: + raise ValueError("The edge sharpness must be greater than or equal to 1.") + + name = ( + name + if isinstance(name, str) + else f"step_{length:.2f}x{width:.2f}x{height:.2f}" + ) + options = {} if options is None else options + + output = cls( + point_position_name=point_position_name, + options=options, + name=name, + ) + + rotation_z = np.array( + [ + [np.cos(orientation), -np.sin(orientation), 0], + [np.sin(orientation), np.cos(orientation), 0], + [0, 0, 1], + ] + ) + point_position_xy = cs.MX.sym(output.get_point_position_name() + "_xy", 2) + + top_expression = cls._top_expression_from_normal( + height, point_position_xy, top_normal_direction + ) + + shape_function = cs.Function( + "step_shape", + [point_position_xy], + [ + (2 / length * point_position_xy[0]) ** (2 * edge_sharpness) + + (2 / width * point_position_xy[1]) ** (2 * edge_sharpness) + ], + [point_position_xy.name()], + ["g"], + options, + ) + top_surface_function = cs.Function( + "step_top_surface", + [point_position_xy], + [top_expression], + [point_position_xy.name()], + ["pi"], + options, + ) + output.set_terrain( + shape_function=shape_function, + top_surface_function=top_surface_function, + sharpness=side_sharpness, + offset=position, + transformation_matrix=rotation_z, + ) + return output + + @classmethod + def cylinder( + cls, + radius: float, + height: float, + side_sharpness: float = 10, + top_normal_direction: np.ndarray = None, + position: np.ndarray = None, + point_position_name: str = None, + options: dict = None, + name: str = None, + ) -> TSmoothTerrain: + name = name if isinstance(name, str) else f"cylinder_{radius:.2f}x{height:.2f}" + options = {} if options is None else options + + output = cls( + point_position_name=point_position_name, + options=options, + name=name, + ) + + point_position_xy = cs.MX.sym(output.get_point_position_name() + "_xy", 2) + + top_expression = cls._top_expression_from_normal( + height, point_position_xy, top_normal_direction + ) + + shape_function = cs.Function( + "cylinder_shape", + [point_position_xy], + [ + (1 / radius * point_position_xy[0]) ** 2 + + (1 / radius * point_position_xy[1]) ** 2 + ], + [point_position_xy.name()], + ["g"], + options, + ) + top_surface_function = cs.Function( + "cylinder_top_surface", + [point_position_xy], + [top_expression], + [point_position_xy.name()], + ["pi"], + options, + ) + output.set_terrain( + shape_function=shape_function, + top_surface_function=top_surface_function, + sharpness=side_sharpness, + offset=position, + ) + return output + + @classmethod + def plane( + cls, + normal_direction: np.ndarray = None, + position: np.ndarray = None, + point_position_name: str = None, + options: dict = None, + name: str = None, + ) -> TSmoothTerrain: + name = ( + name + if isinstance(name, str) + else f"plane_{normal_direction[0]:.2f}x{normal_direction[1]:.2f}" + f"x{normal_direction[2]:.2f}" + ) + options = {} if options is None else options + + output = cls( + point_position_name=point_position_name, + options=options, + name=name, + ) + + point_position_xy = cs.MX.sym(output.get_point_position_name() + "_xy", 2) + + top_expression = cls._top_expression_from_normal( + 0, point_position_xy, normal_direction + ) + + shape_function = cs.Function( + "plane_shape", + [point_position_xy], + [0], + [point_position_xy.name()], + ["g"], + options, + ) + top_surface_function = cs.Function( + "plane_top_surface", + [point_position_xy], + [top_expression], + [point_position_xy.name()], + ["pi"], + options, + ) + output.set_terrain( + shape_function=shape_function, + top_surface_function=top_surface_function, + sharpness=1, + offset=position, + ) + return output + if __name__ == "__main__": viz_settings = TerrainVisualizerSettings() - rotation_x = np.array( - [ - [np.cos(np.pi / 4), -np.sin(np.pi / 4), 0], - [np.sin(np.pi / 4), np.cos(np.pi / 4), 0], - [0, 0, 1], - ] + viz_settings.terrain = ( + SmoothTerrain.step( + length=1.0, + width=0.5, + height=0.5, + position=np.array([-0.5, -0.5, 0.0]), + orientation=np.pi / 4, + top_normal_direction=np.array([0.5, 1.0, 1.0]), + ) + + SmoothTerrain.cylinder( + radius=0.5, height=1.0, position=np.array([0.8, 0.5, 0.0]) + ) + + SmoothTerrain.step( + length=0.5, + width=0.5, + height=0.5, + edge_sharpness=2, + position=np.array([-0.5, 0.5, 0.0]), + ) + + SmoothTerrain.plane( + normal_direction=np.array([0.5, 0.1, 1.0]), + position=np.array([0.0, 0.0, 0.0]), + ) ) - viz_settings.terrain = SmoothTerrain( - transformation_matrix=rotation_x, offset=np.array([-0.5, -0.5, 0.0]) - ) + SmoothTerrain(offset=np.array([0.8, 0.5, 0.0])) viz_settings.overwrite_terrain_files = True viz_settings.draw_terrain_frames = True viz = TerrainVisualizer(viz_settings) From 6b483d4058b8d4b3ff5fc7e2250f67bc8a627bb6 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 14:38:13 +0100 Subject: [PATCH 096/106] Added possibility to set the name in the terrain descriptor --- src/hippopt/robot_planning/utilities/terrain_descriptor.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/utilities/terrain_descriptor.py b/src/hippopt/robot_planning/utilities/terrain_descriptor.py index fbfd3065..a1198bd0 100644 --- a/src/hippopt/robot_planning/utilities/terrain_descriptor.py +++ b/src/hippopt/robot_planning/utilities/terrain_descriptor.py @@ -26,7 +26,7 @@ def __post_init__( ): self.change_options(point_position_name, options) if name is not None: - self._name = name + self.set_name(name) def change_options( self, point_position_name: str = None, options: dict = None, **_ @@ -35,6 +35,7 @@ def change_options( self._options = options if point_position_name is not None: self._point_position_name = point_position_name + self.invalidate_functions() @abc.abstractmethod def create_height_function(self) -> cs.Function: @@ -98,6 +99,9 @@ def orientation_function(self) -> cs.Function: def get_point_position_name(self) -> str: return self._point_position_name + def set_name(self, name: str) -> None: + self._name = name + def get_name(self) -> str: return self._name if isinstance(self._name, str) else self.__class__.__name__ From 94a2e8335f74ce89480f184200d788e953f4b0a5 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 15:06:09 +0100 Subject: [PATCH 097/106] Fixed bug in humanoid_state_visualizer.py Introduced with https://github.com/ami-iit/hippopt/commit/45db9373888b7c73051c2ce586c5beadbb16a99d --- .../robot_planning/utilities/humanoid_state_visualizer.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py index d2480fc7..f5086878 100644 --- a/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py +++ b/src/hippopt/robot_planning/utilities/humanoid_state_visualizer.py @@ -186,10 +186,12 @@ def _update_clone(self, index: int, state: HumanoidState) -> None: point_force = ( np.array(point.f).flatten() * self._settings.contact_force_scaling - ) + ).flatten() self._viz.set_arrow_transform( - origin=point.p, vector=point_force, shape_name=f"[{index}]f_{i}" + origin=np.array(point.p).flatten(), + vector=point_force, + shape_name=f"[{index}]f_{i}", ) def _visualize_single_state( From c704337640225d83496e80c5128b7c02b39c0232 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 15:08:54 +0100 Subject: [PATCH 098/106] Added a detail on why the rotations are limited in the SmoothTerrain --- src/hippopt/robot_planning/utilities/smooth_terrain.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index a400429f..c6dac66f 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -50,7 +50,8 @@ class SmoothTerrain(TerrainDescriptor): the transformation matrix, and [x_offset, y_offset, z_offset]^T is the offset. When applying a transformation matrix, it is also possible to rotate the terrain. Because of the choice of the projection method, the modified z axis needs to be - parallel to the original one (i.e. no rotations around x or y are allowed). + parallel to the original one (i.e. no rotations around x or y are allowed), + otherwise there are issues in the computation of the normal direction. """ From bea4dfa3e04cb9655412dce1c468a570e0b3f9ac Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Thu, 4 Jan 2024 16:33:02 +0100 Subject: [PATCH 099/106] Fixed a bug in the footsteps interpolation --- src/hippopt/robot_planning/utilities/interpolators.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py index 62163eee..a9a74ab6 100644 --- a/src/hippopt/robot_planning/utilities/interpolators.py +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -262,7 +262,7 @@ def append_swing_phase( next_phase = phases_copy[i + 1] stance_points = int( - np.ceil((phase.deactivation_time - phase.activation_time) / dt) + np.ceil((phase.deactivation_time - max(phase.activation_time, t0)) / dt) ) stance_points = min(stance_points, remaining_points) From 8b37307c4d39b92222ddaec62349b83908775b96 Mon Sep 17 00:00:00 2001 From: Stefano Date: Thu, 11 Jan 2024 12:10:30 +0100 Subject: [PATCH 100/106] The terrain was not correctly passed in the contact state plotter --- .../utilities/foot_contact_state_plotter.py | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py index f8b370c7..fe978282 100644 --- a/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py +++ b/src/hippopt/robot_planning/utilities/foot_contact_state_plotter.py @@ -54,11 +54,12 @@ def __post_init__( else None ) - self.terrain = ( - input_terrain - if isinstance(input_terrain, TerrainDescriptor) - else PlanarTerrain() - ) + if not isinstance(self.terrain, TerrainDescriptor): + self.terrain = ( + input_terrain + if isinstance(input_terrain, TerrainDescriptor) + else PlanarTerrain() + ) class ContactPointStatePlotter: @@ -245,7 +246,7 @@ def _create_complementarity_plot( ContactPointStatePlotter( ContactPointStatePlotterSettings( input_complementarity_axes=[el, last_plot], - terrain=terrain, + input_terrain=terrain, ) ) for row in axes_list From df7a3faaeb6bd69d0434984f8f83a8dda89d82de Mon Sep 17 00:00:00 2001 From: Stefano Date: Wed, 17 Jan 2024 11:24:33 +0100 Subject: [PATCH 101/106] Improved documentation of smooth terrain --- .../robot_planning/utilities/smooth_terrain.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/smooth_terrain.py b/src/hippopt/robot_planning/utilities/smooth_terrain.py index c6dac66f..ceef2530 100644 --- a/src/hippopt/robot_planning/utilities/smooth_terrain.py +++ b/src/hippopt/robot_planning/utilities/smooth_terrain.py @@ -21,17 +21,17 @@ class SmoothTerrain(TerrainDescriptor): The height is defined as follows: h(x, y) = exp(−g(x, y)^(2s)) π(x, y). - Here, g(x, y) ≥ 0 is the equation of a closed curve in the xy-plane, - and π(x, y) is the equation of a plane in the xy-plane, defining the shape of the - terrain when exp(−g(x, y)^(2s)) = 1, i.e. when g(x, y) = 0. - The parameter s ≥ 1 controls the smoothness of the terrain. + Here, g(x, y) is the equation of a curve in the xy-plane, and π(x, y) is the + equation of a plane in the xy-plane, defining the shape of the terrain when + exp(−g(x, y)^(2s)) = 1, i.e. when g(x, y) = 0. The parameter s ≥ 1 controls the + smoothness of the terrain. Independently of the value of s, the value of exp(−g(.)^(2s)) is always passing - through 1 when g(x,y) = 0, and through 1/e when g(x,y) = 1. Then, it will - tend to zero as g(x, y) → ∞. The parameter s controls how fast exp(−g(.)^(2s)) tends - to zero as g(x, y) grows. By multiplying times the equation of a plane, i.e. + through 1 when g(x,y) = 0, and through 1/e when |g(x,y)| = 1. Then, it will + tend to zero as |g(x, y)| → ∞. The parameter s controls how fast exp(−g(.)^(2s)) + tends to zero as |g(x, y)| grows. By multiplying times the equation of a plane, i.e. π(x, y), we can control the inclination of the top surface when g(x, y) = 0. - Instead, g(x, y) = 1 controls the shape of the terrain at height 1/e * π(x, y). + Instead, |g(x, y)| = 1 controls the shape of the terrain at height 1/e * π(x, y). For example, to define a classical step with a square base of dimension l, we can use: From 18b27a625c8ccbf7df07460339c7d195a5d683f6 Mon Sep 17 00:00:00 2001 From: Stefano Dafarra Date: Fri, 19 Jan 2024 10:17:22 +0100 Subject: [PATCH 102/106] Apply suggestions from code review to centroidal expressions Co-authored-by: Diego Ferigo --- .../robot_planning/expressions/centroidal.py | 42 ++++++++++--------- 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/centroidal.py b/src/hippopt/robot_planning/expressions/centroidal.py index 010eff85..998ec4ed 100644 --- a/src/hippopt/robot_planning/expressions/centroidal.py +++ b/src/hippopt/robot_planning/expressions/centroidal.py @@ -14,12 +14,17 @@ def centroidal_dynamics_with_point_forces( ) -> cs.Function: options = {} if options is None else options - if point_position_names is None: - point_position_names = [] - for i in range(number_of_points): - point_position_names.append(f"p{i}") + point_position_names = ( + point_position_names + if point_position_names is not None + else [f"p{i}" for i in range(number_of_points)] + ) - assert len(point_position_names) == number_of_points + if len(point_position_names) != number_of_points: + raise ValueError( + f"Expected {number_of_points} point position names," + f" got {len(point_position_names)}" + ) if point_force_names is None: point_force_names = [] @@ -44,20 +49,19 @@ def centroidal_dynamics_with_point_forces( p = [] f = [] - for i in range(number_of_points): - p.append(cs.MX.sym(point_position_names[i], 3)) - input_vars.append(p[i]) - f.append(cs.MX.sym(point_force_names[i], 3)) - input_vars.append(f[i]) - - input_names = [] - for var in input_vars: - input_names.append(var.name()) - - h_g = m @ g - - for i in range(number_of_points): - h_g = h_g + cs.vertcat(f[i], cs.cross(p[i] - x, f[i])) + for point_position_name, point_force_name in zip( + point_position_names, point_force_names + ): + p.append(cs.MX.sym(point_position_name, 3)) + input_vars.append(p[-1]) + f.append(cs.MX.sym(point_force_name, 3)) + input_vars.append(f[-1]) + + input_names = [var.name() for var in input_vars] + + h_g = m * g + cs.sum2( + cs.horzcat(*[cs.vertcat(fi, cs.cross(pi - x, fi)) for fi, pi in zip(f, p)]) + ) return cs.Function( "centroidal_dynamics_with_point_forces", From f01632ba1e398221629c840f8c28b2193043799b Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 19 Jan 2024 11:52:27 +0100 Subject: [PATCH 103/106] Improved OptiCallback. opti.debug is called only once --- src/hippopt/base/opti_callback.py | 34 +++++++++++++++++++++---------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/hippopt/base/opti_callback.py b/src/hippopt/base/opti_callback.py index 3cfc2576..d4c1aa17 100644 --- a/src/hippopt/base/opti_callback.py +++ b/src/hippopt/base/opti_callback.py @@ -27,6 +27,7 @@ class CallbackCriterion(abc.ABC): def __init__(self) -> None: """""" self.opti = None + self.opti_debug = None @abc.abstractmethod def satisfied(self) -> bool: @@ -69,6 +70,10 @@ def set_opti(self, opti: cs.Opti) -> None: self.opti = weakref.proxy(opti) self.reset() + def update_opti_debug(self, opti_debug: cs.OptiAdvanced) -> None: + """""" + self.opti_debug = opti_debug + class BestCost(CallbackCriterion): """""" @@ -107,7 +112,7 @@ def update(self) -> None: def _get_current_cost(self) -> float: """""" - return self.opti.debug.value(self.opti.f) + return self.opti_debug.value(self.opti.f) class AcceptableCost(CallbackCriterion): @@ -151,7 +156,7 @@ def update(self) -> None: def _get_current_cost(self) -> float: """""" - return self.opti.debug.value(self.opti.f) + return self.opti_debug.value(self.opti.f) class AcceptablePrimalInfeasibility(CallbackCriterion): @@ -199,7 +204,7 @@ def update(self) -> None: def _get_current_primal_infeasibility(self) -> float: """""" - return self.opti.debug.stats()["iterations"]["inf_pr"][-1] + return self.opti_debug.stats()["iterations"]["inf_pr"][-1] class BestPrimalInfeasibility(CallbackCriterion): @@ -240,7 +245,7 @@ def update(self) -> None: def _get_current_primal_infeasibility(self) -> float: """""" - return self.opti.debug.stats()["iterations"]["inf_pr"][-1] + return self.opti_debug.stats()["iterations"]["inf_pr"][-1] class CombinedCallbackCriterion(CallbackCriterion, abc.ABC): @@ -274,6 +279,13 @@ def set_opti(self, opti: cs.Opti) -> None: self.lhs.set_opti(opti) self.rhs.set_opti(opti) + @final + def update_opti_debug(self, opti_debug: cs.OptiAdvanced) -> None: + """""" + + self.lhs.update_opti_debug(opti_debug) + self.rhs.update_opti_debug(opti_debug) + class OrCombinedCallbackCriterion(CombinedCallbackCriterion): """""" @@ -330,6 +342,8 @@ def __init__( def call(self, i: int) -> None: """""" + opti_debug = self.opti.debug + self.criterion.update_opti_debug(opti_debug) if self.criterion.satisfied(): self.criterion.update() @@ -338,24 +352,22 @@ def call(self, i: int) -> None: _logger.info(f"[i={i}] New best intermediate variables") self.best_iteration = i - self.best_cost = self.opti.debug.value(self.opti.f) + self.best_cost = opti_debug.value(self.opti.f) for variable in self.variables: if self.ignore_map[variable]: continue try: - self.best_objects[variable] = self.opti.debug.value(variable) + self.best_objects[variable] = opti_debug.value(variable) except Exception as err: # noqa self.ignore_map[variable] = True for parameter in self.parameters: if self.ignore_map[parameter]: continue - self.best_objects[parameter] = self.opti.debug.value(parameter) + self.best_objects[parameter] = opti_debug.value(parameter) self.ignore_map[parameter] = True # Parameters are saved only once - self.best_cost_values = { - cost: self.opti.debug.value(cost) for cost in self.cost - } + self.best_cost_values = {cost: opti_debug.value(cost) for cost in self.cost} self.best_constraint_multipliers = { - constraint: self.opti.debug.value(self.opti.dual(constraint)) + constraint: opti_debug.value(self.opti.dual(constraint)) for constraint in self.constraints } From 11e715e617d9e3292c4fb786d6d986e3eaa18e0b Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 19 Jan 2024 14:18:38 +0100 Subject: [PATCH 104/106] Pattern improvements on contact expressions after review --- src/hippopt/robot_planning/expressions/contacts.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/hippopt/robot_planning/expressions/contacts.py b/src/hippopt/robot_planning/expressions/contacts.py index 2d7f3840..1d692aa8 100644 --- a/src/hippopt/robot_planning/expressions/contacts.py +++ b/src/hippopt/robot_planning/expressions/contacts.py @@ -92,13 +92,11 @@ def contact_points_centroid( input_vars = [] p = [] - for i in range(number_of_points): - p.append(cs.MX.sym(point_position_names[i], 3)) - input_vars.append(p[i]) + for point_position_name in point_position_names: + p.append(cs.MX.sym(point_position_name, 3)) + input_vars.append(p[-1]) - input_names = [] - for var in input_vars: - input_names.append(var.name()) + input_names = [var.name() for var in input_vars] centroid = cs.DM.zeros(3, 1) From 12480c5acdb7d51b35e0d7899390ed0f143e90c2 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 19 Jan 2024 15:25:05 +0100 Subject: [PATCH 105/106] Improved pattern on interpolators --- .../robot_planning/utilities/interpolators.py | 59 ++++++++----------- 1 file changed, 23 insertions(+), 36 deletions(-) diff --git a/src/hippopt/robot_planning/utilities/interpolators.py b/src/hippopt/robot_planning/utilities/interpolators.py index a9a74ab6..d639e04b 100644 --- a/src/hippopt/robot_planning/utilities/interpolators.py +++ b/src/hippopt/robot_planning/utilities/interpolators.py @@ -317,14 +317,10 @@ def feet_contact_points_interpolator( assert len(left_output) == len(right_output) == number_of_points - output = [] - for i in range(number_of_points): - output_state = FeetContactPoints() - output_state.left = left_output[i] - output_state.right = right_output[i] - output.append(output_state) - - return output + return [ + FeetContactPoints(left=left, right=right) + for left, right in zip(left_output, right_output) + ] def free_floating_object_state_interpolator( @@ -345,13 +341,12 @@ def free_floating_object_state_interpolator( assert ( len(position_interpolation) == len(quaternion_interpolation) == number_of_points ) - output = [] - for i in range(number_of_points): - output_state = FreeFloatingObjectState() - output_state.position = position_interpolation[i] - output_state.quaternion_xyzw = quaternion_interpolation[i] - output.append(output_state) - return output + return [ + FreeFloatingObjectState(position=position, quaternion_xyzw=quaternion) + for position, quaternion in zip( + position_interpolation, quaternion_interpolation + ) + ] def kinematic_tree_state_interpolator( @@ -370,14 +365,9 @@ def kinematic_tree_state_interpolator( final=final_state.positions, number_of_points=number_of_points, ) - output = [] - for i in range(number_of_points): - output_state = KinematicTreeState( - number_of_joints_state=len(initial_state.positions) - ) - output_state.positions = positions_interpolation[i] - output.append(output_state) - return output + return [ + KinematicTreeState(positions=positions) for positions in positions_interpolation + ] def floating_base_system_state_interpolator( @@ -398,15 +388,10 @@ def floating_base_system_state_interpolator( assert len(base_interpolation) == len(joints_interpolation) == number_of_points - output = [] - for i in range(number_of_points): - output_state = FloatingBaseSystemState( - number_of_joints_state=len(initial_state.joints.positions) - ) - output_state.base = base_interpolation[i] - output_state.joints = joints_interpolation[i] - output.append(output_state) - return output + return [ + FloatingBaseSystemState(base=base, joints=joints) + for base, joints in zip(base_interpolation, joints_interpolation) + ] def humanoid_state_interpolator( @@ -444,13 +429,15 @@ def humanoid_state_interpolator( ) output = [] - for i in range(number_of_points): + for points, kin, com in zip( + contact_points_interpolation, kinematics_interpolation, com_interpolation + ): output_state = HumanoidState( contact_point_descriptors=contact_descriptor, number_of_joints=len(initial_state.kinematics.joints.positions), ) - output_state.contact_points = contact_points_interpolation[i] - output_state.kinematics = kinematics_interpolation[i] - output_state.com = com_interpolation[i] + output_state.contact_points = points + output_state.kinematics = kin + output_state.com = com output.append(output_state) return output From 57e4b49d4ebe8b71b12a4dc36a3f9fdc9cbb7c69 Mon Sep 17 00:00:00 2001 From: Stefano Date: Fri, 19 Jan 2024 15:25:21 +0100 Subject: [PATCH 106/106] Improved patterns on variables --- .../robot_planning/variables/contacts.py | 22 ++++++++----------- .../robot_planning/variables/floating_base.py | 17 ++++++++------ .../robot_planning/variables/humanoid.py | 3 +-- 3 files changed, 20 insertions(+), 22 deletions(-) diff --git a/src/hippopt/robot_planning/variables/contacts.py b/src/hippopt/robot_planning/variables/contacts.py index 1ac58e6c..f1e63a3d 100644 --- a/src/hippopt/robot_planning/variables/contacts.py +++ b/src/hippopt/robot_planning/variables/contacts.py @@ -78,12 +78,8 @@ class ContactPointState(OptimizationObject): ) def __post_init__(self, input_descriptor: ContactPointDescriptor) -> None: - if self.p is None: - self.p = np.zeros(3) - - if self.f is None: - self.f = np.zeros(3) - + self.p = np.zeros(3) if self.p is None else self.p + self.f = np.zeros(3) if self.f is None else self.f if input_descriptor is not None: self.descriptor = input_descriptor @@ -94,11 +90,8 @@ class ContactPointStateDerivative(OptimizationObject): f_dot: StorageType = default_storage_field(OverridableVariable) def __post_init__(self) -> None: - if self.v is None: - self.v = np.zeros(3) - - if self.f_dot is None: - self.f_dot = np.zeros(3) + self.v = np.zeros(3) if self.v is None else self.v + self.f_dot = np.zeros(3) if self.f_dot is None else self.f_dot TFootContactState = TypeVar("TFootContactState", bound="FootContactState") @@ -154,10 +147,13 @@ class FootContactPhaseDescriptor: deactivation_time: float | None = dataclasses.field(default=None) def __post_init__(self) -> None: - if self.transform is None: - self.transform = liecasadi.SE3.from_translation_and_rotation( + self.transform = ( + liecasadi.SE3.from_translation_and_rotation( cs.DM.zeros(3), liecasadi.SO3.Identity() ) + if self.transform is None + else self.transform + ) if self.force is None: self.force = np.zeros(3) self.force[2] = 100 diff --git a/src/hippopt/robot_planning/variables/floating_base.py b/src/hippopt/robot_planning/variables/floating_base.py index 9af82245..30b4c2f3 100644 --- a/src/hippopt/robot_planning/variables/floating_base.py +++ b/src/hippopt/robot_planning/variables/floating_base.py @@ -19,8 +19,7 @@ class FreeFloatingObjectState(OptimizationObject): quaternion_xyzw: StorageType = default_storage_field(OverridableVariable) def __post_init__(self): - if self.position is None: - self.position = np.zeros(3) + self.position = np.zeros(3) if self.position is None else self.position if self.quaternion_xyzw is None: self.quaternion_xyzw = np.zeros(4) self.quaternion_xyzw[3] = 1.0 @@ -32,11 +31,15 @@ class FreeFloatingObjectStateDerivative(OptimizationObject): quaternion_velocity_xyzw: StorageType = default_storage_field(OverridableVariable) def __post_init__(self): - if self.linear_velocity is None: - self.linear_velocity = np.zeros(3) + self.linear_velocity = ( + np.zeros(3) if self.linear_velocity is None else self.linear_velocity + ) - if self.quaternion_velocity_xyzw is None: - self.quaternion_velocity_xyzw = np.zeros(4) + self.quaternion_velocity_xyzw = ( + np.zeros(4) + if self.quaternion_velocity_xyzw is None + else self.quaternion_velocity_xyzw + ) @dataclasses.dataclass @@ -53,7 +56,7 @@ class KinematicTreeState(OptimizationObject): number_of_joints_state: dataclasses.InitVar[int] = dataclasses.field(default=0) def __post_init__(self, number_of_joints_state: int): - if number_of_joints_state is not None: + if number_of_joints_state is not None and self.positions is None: self.positions = np.zeros(number_of_joints_state) diff --git a/src/hippopt/robot_planning/variables/humanoid.py b/src/hippopt/robot_planning/variables/humanoid.py index 47c9fee2..884adb26 100644 --- a/src/hippopt/robot_planning/variables/humanoid.py +++ b/src/hippopt/robot_planning/variables/humanoid.py @@ -53,5 +53,4 @@ def __post_init__( self.kinematics = FloatingBaseSystemState( number_of_joints_state=number_of_joints ) - if self.com is None: - self.com = np.zeros(3) + self.com = np.zeros(3) if self.com is None else self.com