Humanoid details

habitat-lab/habitat/articulated_agent_controllers/__init__.py

@@ -4,10 +4,20 @@
 # This source code is licensed under the MIT license found in the
 # LICENSE file in the root directory of this source tree.
 
+from habitat.articulated_agent_controllers.humanoid_base_controller import (
+    HumanoidBaseController,
+    Motion,
+    Pose,
+)
 from habitat.articulated_agent_controllers.humanoid_rearrange_controller import (
     HumanoidRearrangeController,
 )
+from habitat.articulated_agent_controllers.humanoid_seq_pose_controller import (
+    HumanoidSeqPoseController,
+)
 
 __all__ = [
+    "HumanoidBaseController",
     "HumanoidRearrangeController",
+    "HumanoidSeqPoseController",
 ]

habitat-lab/habitat/articulated_agent_controllers/humanoid_base_controller.py

@@ -0,0 +1,93 @@
+#!/usr/bin/env python3
+
+# Copyright (c) Meta Platforms, Inc. and its affiliates.
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+
+import magnum as mn
+import numpy as np
+
+
+# TODO: The implementation here assumes a SMPLX representation of humanoids
+# where all joints are representated as quaternions. In future PRs we need
+# to abstract this class to support other kinds of joints.
+class Pose:
+    def __init__(self, joints_quat, root_transform):
+        """
+        Contains a single humanoid pose
+            :param joints_quat: list or array of num_joints * 4 elements, with the rotation quaternions
+            :param root_transform: Matrix4 with the root trasnform.
+        """
+        self.joints = list(joints_quat)
+        self.root_transform = root_transform
+
+
+class Motion:
+    """
+    Contains a sequential motion, corresponding to a sequence of poses
+        :param joints_quat_array: num_poses x num_joints x 4 array, containing the join orientations
+        :param transform_array: num_poses x 4 x 4 array, containing the root transform
+        :param displacement: on each pose, how much forward displacement was there?
+            Used to measure how many poses we should advance to move a cerain amount
+        :param fps: the FPS at which the motion was recorded
+    """
+
+    def __init__(self, joints_quat_array, transform_array, displacement, fps):
+        num_poses = joints_quat_array.shape[0]
+        self.num_poses = num_poses
+        poses = []
+        for index in range(num_poses):
+            pose = Pose(
+                joints_quat_array[index].reshape(-1),
+                mn.Matrix4(transform_array[index]),
+            )
+            poses.append(pose)
+
+        self.poses = poses
+        self.fps = fps
+        self.displacement = displacement
+
+
+class HumanoidBaseController:
+    """
+    Generic class to replay SMPL-X motions
+        :param motion_fps: the FPS at which we should be playing the motion.
+        :param base_offset: what is the offset between the root of the character and their feet.
+    """
+
+    def __init__(
+        self,
+        motion_fps=30,
+        base_offset=(0, 0.9, 0),
+    ):
+        self.base_offset = mn.Vector3(base_offset)
+        self.motion_fps = motion_fps
+
+        # These two matrices store the global transformation of the base
+        # as well as the transformation caused by the walking gait
+        # We initialize them to identity
+        self.obj_transform_offset = mn.Matrix4()
+        self.obj_transform_base = mn.Matrix4()
+        self.joint_pose = []
+
+    def reset(self, base_transformation: mn.Matrix4) -> None:
+        """Reset the joints on the human. (Put in rest state)"""
+        self.obj_transform_offset = mn.Matrix4()
+        self.obj_transform_base = base_transformation
+        self.prev_orientation = base_transformation.transform_vector(
+            mn.Vector3(1.0, 0.0, 0.0)
+        )
+
+    def get_pose(self):
+        """
+        Obtains the controller joints, offset and base transform in a vectorized form so that it can be passed
+        as an argument to HumanoidJointAction
+        """
+        obj_trans_offset = np.asarray(
+            self.obj_transform_offset.transposed()
+        ).flatten()
+        obj_trans_base = np.asarray(
+            self.obj_transform_base.transposed()
+        ).flatten()
+        return self.joint_pose + list(obj_trans_offset) + list(obj_trans_base)

habitat-lab/habitat/articulated_agent_controllers/humanoid_rearrange_controller.py

@@ -11,43 +11,11 @@
 import magnum as mn
 import numpy as np
 
-
-class Pose:
-    def __init__(self, joints_quat, root_transform):
-        """
-        Contains a single humanoid pose
-            :param joints_quat: list or array of num_joints * 4 elements, with the rotation quaternions
-            :param root_transform: Matrix4 with the root trasnform.
-        """
-        self.joints = list(joints_quat)
-        self.root_transform = root_transform
-
-
-class Motion:
-    """
-    Contains a sequential motion, corresponding to a sequence of poses
-        :param joints_quat_array: num_poses x num_joints x 4 array, containing the join orientations
-        :param transform_array: num_poses x 4 x 4 array, containing the root transform
-        :param displacement: on each pose, how much forward displacement was there?
-            Used to measure how many poses we should advance to move a cerain amount
-        :param fps: the FPS at which the motion was recorded
-    """
-
-    def __init__(self, joints_quat_array, transform_array, displacement, fps):
-        num_poses = joints_quat_array.shape[0]
-        self.num_poses = num_poses
-        poses = []
-        for index in range(num_poses):
-            pose = Pose(
-                joints_quat_array[index].reshape(-1),
-                mn.Matrix4(transform_array[index]),
-            )
-            poses.append(pose)
-
-        self.poses = poses
-        self.fps = fps
-        self.displacement = displacement
-
+from habitat.articulated_agent_controllers import (
+    HumanoidBaseController,
+    Motion,
+    Pose,
+)
 
 MIN_ANGLE_TURN = 5  # If we turn less than this amount, we can just rotate the base and keep walking motion the same as if we had not rotated
 TURNING_STEP_AMOUNT = (
@@ -59,24 +27,26 @@ def __init__(self, joints_quat_array, transform_array, displacement, fps):
 )
 
 
-class HumanoidRearrangeController:
+class HumanoidRearrangeController(HumanoidBaseController):
     """
     Humanoid Controller, converts high level actions such as walk, or reach into joints positions
         :param walk_pose_path: file containing the walking poses we care about.
-        :param draw_fps: the FPS at which we should be advancing the pose.
+        :param motion_fps: the FPS at which we should be advancing the pose.
         :base_offset: what is the offset between the root of the character and their feet.
     """
 
     def __init__(
         self,
         walk_pose_path,
-        draw_fps=30,
+        motion_fps=30,
         base_offset=(0, 0.9, 0),
     ):
+        self.obj_transform_base = mn.Matrix4()
+        super().__init__(motion_fps, base_offset)
+
         self.min_angle_turn = MIN_ANGLE_TURN
         self.turning_step_amount = TURNING_STEP_AMOUNT
         self.threshold_rotate_not_move = THRESHOLD_ROTATE_NOT_MOVE
-        self.base_offset = mn.Vector3(base_offset)
 
         if not os.path.isfile(walk_pose_path):
             raise RuntimeError(
@@ -98,18 +68,11 @@ def __init__(
             walk_data["stop_pose"]["joints"].reshape(-1),
             mn.Matrix4(walk_data["stop_pose"]["transform"]),
         )
-        self.draw_fps = draw_fps
+        self.motion_fps = motion_fps
         self.dist_per_step_size = (
             self.walk_motion.displacement[-1] / self.walk_motion.num_poses
         )
 
-        # These two matrices store the global transformation of the base
-        # as well as the transformation caused by the walking gait
-        # We initialize them to identity
-        self.obj_transform_offset = mn.Matrix4()
-        self.obj_transform_base = mn.Matrix4()
-        self.joint_pose = []
-
         self.prev_orientation = None
         self.walk_mocap_frame = 0
 
@@ -147,20 +110,12 @@ def set_framerate_for_linspeed(self, lin_speed, ang_speed, ctrl_freq):
         seconds_per_step = 1.0 / ctrl_freq
         meters_per_step = lin_speed * seconds_per_step
         frames_per_step = meters_per_step / self.dist_per_step_size
-        self.draw_fps = self.walk_motion.fps / frames_per_step
+        self.motion_fps = self.walk_motion.fps / frames_per_step
         rotate_amount = ang_speed * seconds_per_step
         rotate_amount = rotate_amount * 180.0 / np.pi
         self.turning_step_amount = rotate_amount
         self.threshold_rotate_not_move = rotate_amount
 
-    def reset(self, base_transformation) -> None:
-        """Reset the joints on the human. (Put in rest state)"""
-        self.obj_transform_offset = mn.Matrix4()
-        self.obj_transform_base = base_transformation
-        self.prev_orientation = base_transformation.transform_vector(
-            mn.Vector3(1.0, 0.0, 0.0)
-        )
-
     def calculate_stop_pose(self):
         """
         Calculates a stop, standing pose
@@ -226,7 +181,7 @@ def calculate_walk_pose(
         self.prev_orientation = forward_V
 
         # Step size according to the FPS
-        step_size = int(self.walk_motion.fps / self.draw_fps)
+        step_size = int(self.walk_motion.fps / self.motion_fps)
 
         if did_rotate:
             # When we rotate, we allow some movement
@@ -493,16 +448,3 @@ def calculate_reach_pose(self, obj_pos: mn.Vector3, index_hand=0):
         )
 
         self.joint_pose = curr_poses
-
-    def get_pose(self):
-        """
-        Obtains the controller joints, offset and base transform in a vectorized form so that it can be passed
-        as an argument to HumanoidJointAction
-        """
-        obj_trans_offset = np.asarray(
-            self.obj_transform_offset.transposed()
-        ).flatten()
-        obj_trans_base = np.asarray(
-            self.obj_transform_base.transposed()
-        ).flatten()
-        return self.joint_pose + list(obj_trans_offset) + list(obj_trans_base)

habitat-lab/habitat/articulated_agent_controllers/humanoid_seq_pose_controller.py

@@ -0,0 +1,113 @@
+#!/usr/bin/env python3
+
+# Copyright (c) Meta Platforms, Inc. and its affiliates.
+# This source code is licensed under the MIT license found in the
+# LICENSE file in the root directory of this source tree.
+
+import os
+
+import magnum as mn
+import numpy as np
+
+from habitat.articulated_agent_controllers import (
+    HumanoidBaseController,
+    Motion,
+)
+
+
+class HumanoidSeqPoseController(HumanoidBaseController):
+    """
+    Humanoid Seq Pose Controller, replays a sequence of humanoid poses.
+        :param motion_pose_path: file containing the motion poses we want to play.
+        :param motion_fps: the FPS at which we should be advancing the pose.
+        :param base_offset: what is the offset between the root of the character and their feet.
+    """
+
+    def __init__(
+        self,
+        motion_pose_path,
+        motion_fps=30,
+        base_offset=(0, 0.9, 0),
+    ):
+        super().__init__(motion_fps, base_offset)
+
+        if not os.path.isfile(motion_pose_path):
+            raise RuntimeError(
+                f"Path does {motion_pose_path} not exist. Reach out to the paper authors to obtain this data."
+            )
+
+        motion_info = np.load(motion_pose_path, allow_pickle=True)
+        motion_info = motion_info["pose_motion"]
+        self.humanoid_motion = Motion(
+            motion_info["joints_array"],
+            motion_info["transform_array"],
+            motion_info["displacement"],
+            motion_info["fps"],
+        )
+        self.motion_frame = 0
+        self.ref_pose = mn.Matrix4()
+        self.first_pose = mn.Matrix4(
+            self.humanoid_motion.poses[0].root_transform
+        )
+        self.step_size = int(self.humanoid_motion.fps / self.motion_fps)
+
+    def reset(self, base_transformation: mn.Matrix4) -> None:
+        """Reset the joints on the human. (Put in rest state)"""
+        super().reset(base_transformation)
+        self.motion_frame = 0
+
+        # The first pose of the motion file will be set at base_transformation
+        self.ref_pose = base_transformation
+
+        self.calculate_pose()
+
+    def next_pose(self, cycle=False) -> None:
+        """
+        Move to the next pose in the motion sequence
+            :param cycle: boolean indicating whether we should stop or cycle when reaching the last pose
+        """
+
+        if cycle:
+            self.motion_frame = (
+                self.motion_frame + self.step_size
+            ) % self.humanoid_motion.num_poses
+        else:
+            self.motion_frame = min(
+                self.motion_frame + 1, self.humanoid_motion.num_poses - 1
+            )
+
+    def prev_pose(self, cycle=False) -> None:
+        """
+        Move to the previous pose in the motion sequence
+            :param cycle: boolean indicating whether we should stop or cycle when reaching the first pose
+        """
+
+        if cycle:
+            self.motion_frame = (
+                self.motion_frame - self.step_size
+            ) % self.humanoid_motion.num_poses
+        else:
+            self.motion_frame = max(0, self.motion_frame - 1)
+
+    def calculate_pose(self, advance_pose=False) -> None:
+        """
+        Set the joint transforms according to the current frame
+            :param advance_pose: whether this function should move to the next pose
+        """
+        curr_transform = mn.Matrix4(
+            self.humanoid_motion.poses[self.motion_frame].root_transform
+        )
+        curr_transform.translation = (
+            curr_transform.translation
+            - self.first_pose.translation
+            + self.ref_pose.translation
+        )
+        curr_transform.translation = curr_transform.translation - mn.Vector3(
+            0, 0.9, 0
+        )
+        curr_poses = self.humanoid_motion.poses[self.motion_frame].joints
+        self.obj_transform_offset = curr_transform
+        self.joint_pose = curr_poses
+
+        if advance_pose:
+            self.next_pose()