Added base position derivative

src/hippopt/__init__.py

@@ -28,3 +28,4 @@
  centroidal_dynamics_with_point_forces,
  com_dynamics_from_momentum,
 )
+from .robot_planning.dynamics.frames import base_position_derivative

src/hippopt/robot_planning/dynamics/__init__.py

@@ -1 +1 @@
-from . import centroidal
+from . import centroidal, frames

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 = []
 
@@ -54,6 +53,8 @@ def centroidal_dynamics_with_point_forces(
 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)
 

src/hippopt/robot_planning/dynamics/frames.py

@@ -0,0 +1,26 @@
+import casadi as cs
+import liecasadi
+
+
+def base_position_derivative(
+ base_quaternion_name: str = "base_quaternion",
+ base_left_trivialized_linear_velocity: str = "base_lin_velocity",
+ options: dict = None,
+) -> cs.Function:
+ options = {} if options is None else options
+ quaternion = cs.MX.sym(base_quaternion_name, 4)
+ vel = cs.MX.sym(base_left_trivialized_linear_velocity, 3)
+
+ normalized_quaternion = liecasadi.Quaternion(xyzw=quaternion).normalize()
+ base_rotation = liecasadi.SO3.from_quat(normalized_quaternion.xyzw).as_matrix()
+
+ p_dot = base_rotation @ vel
+
+ return cs.Function(
+ "base_position_derivative",
+ [quaternion, vel],
+ [p_dot],
+ [base_quaternion_name, base_left_trivialized_linear_velocity],
+ ["base_position_derivative"],
+ options,
+ )