Added base class for a generic terrain description and a PlanarTerrain

src/hippopt/__init__.py

@@ -1,4 +1,3 @@
-from . import base, integrators
 from .base.dynamics import Dynamics, TypedDynamics, dot
 from .base.multiple_shooting_solver import MultipleShootingSolver
 from .base.opti_solver import OptiFailure, OptiSolver
@@ -22,16 +21,3 @@
  step,
 )
 from .base.variable import TVariable, Variable
-from .robot_planning.dynamics.centroidal import (
- centroidal_dynamics_with_point_forces,
- com_dynamics_from_momentum,
-)
-from .robot_planning.expressions.kinematics import (
- center_of_mass_position_from_kinematics,
- centroidal_momentum_from_kinematics,
- point_position_from_kinematics,
-)
-from .robot_planning.utilities.quaternion import (
- quaternion_xyzw_normalization,
- quaternion_xyzw_velocity_to_right_trivialized_angular_velocity,
-)

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

src/hippopt/robot_planning/utilities/__init__.py

@@ -1 +1 @@
-from . import quaternion
+from . import quaternion, terrain_descriptor

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,
+ )

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