forked from facebookresearch/habitat-lab
-
Notifications
You must be signed in to change notification settings - Fork 2
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Humanoid details (facebookresearch#1703)
* updates * any motion with humanoids * update * update * Update habitat-lab/habitat/articulated_agents/humanoids/README.md Co-authored-by: Alexander Clegg <[email protected]> * remove title without content * Update habitat-lab/habitat/articulated_agents/humanoids/README.md Co-authored-by: Alexander Clegg <[email protected]> * Update habitat-lab/habitat/articulated_agents/humanoids/README.md Co-authored-by: Alexander Clegg <[email protected]> * added todo comment --------- Co-authored-by: Alexander Clegg <[email protected]>
- Loading branch information
1 parent
8b75a68
commit 9596030
Showing
7 changed files
with
614 additions
and
75 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
93 changes: 93 additions & 0 deletions
93
habitat-lab/habitat/articulated_agent_controllers/humanoid_base_controller.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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) |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
113 changes: 113 additions & 0 deletions
113
habitat-lab/habitat/articulated_agent_controllers/humanoid_seq_pose_controller.py
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -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() |
Oops, something went wrong.