From 99e506bf0e98f097e1a973e927c40527eb83a94d Mon Sep 17 00:00:00 2001
From: Deepak Gopinath <dgopinath@fb.com>
Date: Mon, 20 Dec 2021 08:52:45 -0800
Subject: [PATCH] Ops to fix height and translate motion to origin

---
 fairmotion/ops/motion.py | 24 ++++++++++++++++++++++++
 1 file changed, 24 insertions(+)

diff --git a/fairmotion/ops/motion.py b/fairmotion/ops/motion.py
index cf3faab..70680aa 100644
--- a/fairmotion/ops/motion.py
+++ b/fairmotion/ops/motion.py
@@ -5,6 +5,7 @@
 
 from fairmotion.core import motion as motion_class
 from fairmotion.ops import conversions, math, quaternion
+from fairmotion.utils import constants, utils
 
 
 def append(motion1, motion2):
@@ -199,3 +200,26 @@ def position_wrt_root(motion):
     # Subtract root position from all joint positions
     matrix = matrix - matrix[:, np.newaxis, 0]
     return matrix
+
+
+def fix_height(motion, axis_up="y"):
+    """
+    Translates global root position such that the feet do not penetrate the
+    floor. It calculates lowest global joint position along `axis_up` and
+    offsets the root position by that magnitude.
+    """
+    axis = utils.str_to_axis(axis_up)
+    pos = motion.positions(local=False)[..., axis.argmax()]
+    min_height = np.min(pos)
+    fixed_motion = translate(motion, -min_height*axis, local=False)
+    return fixed_motion
+
+def start_from_origin(motion, axis_up="y"):
+    axis = utils.str_to_axis(axis_up)
+    ground_axis_indices = [i for i in range(3) if i != axis.argmax()]
+    # Global position of root in 0th frame
+    pos = conversions.T2p(motion.poses[0].get_root_transform())
+    # Zero out offset along axis_up 
+    pos -= axis * pos
+    translated_motion = translate(motion, -pos)
+    return translated_motion