Humanoids pr

examples/interactive_play.py

@@ -87,7 +87,12 @@ def step_env(env, action_name, action_args):
 
 
 def get_input_vel_ctlr(
-    skip_pygame, arm_action, env, not_block_input, agent_to_control
+    skip_pygame,
+    arm_action,
+    env,
+    not_block_input,
+    agent_to_control,
+    use_joint_control,
 ):
     if skip_pygame:
         return step_env(env, "empty", {}), None, False
@@ -98,10 +103,15 @@ def get_input_vel_ctlr(
     else:
         agent_k = ""
     arm_action_name = f"{agent_k}arm_action"
-    base_action_name = f"{agent_k}base_velocity"
-    arm_key = "arm_action"
-    grip_key = "grip_action"
-    base_key = "base_vel"
+
+    if use_joint_control:
+        base_action_name = f"{agent_k}humanjoint_action"
+        base_key = "human_joints_trans"
+    else:
+        base_action_name = f"{agent_k}base_velocity"
+        arm_key = "arm_action"
+        grip_key = "grip_action"
+        base_key = "base_vel"
 
     if arm_action_name in env.action_space.spaces:
         arm_action_space = env.action_space.spaces[arm_action_name].spaces[
@@ -252,6 +262,42 @@ def get_input_vel_ctlr(
             logger.info("[play.py]: Snapping")
             magic_grasp = 1
 
+    if use_joint_control:
+        # Add random noise to human arms but keep global transform
+        (
+            new_joints,
+            root_trans,
+        ) = env._sim.articulated_agent.get_joint_transform()
+        num_joints = len(new_joints) // 4
+        root_trans = np.array(root_trans)
+        new_joints_quat = [
+            mn.Quaternion(
+                mn.Vector3(new_joints[(4 * index) : (4 * index + 3)]),
+                new_joints[4 * index + 3],
+            )
+            for index in range(num_joints)
+        ]
+        rotated_joints_quat = []
+        for index, joint_quat in enumerate(new_joints_quat):
+            random_vec = np.random.rand(3)
+            random_angle = np.random.rand() * 360 / 60
+            rotation_quat = mn.Quaternion.rotation(
+                mn.Rad(random_angle), mn.Vector3(random_vec).normalized()
+            )
+            new_quat = joint_quat
+            if index > 10:
+                new_quat = joint_quat * rotation_quat
+            rotated_joints_quat.append(new_quat)
+        new_joints = np.concatenate(
+            [
+                np.array(list(quat.vector) + [quat.scalar])
+                for quat in rotated_joints_quat
+            ]
+        )
+        base_action = np.concatenate(
+            [new_joints.reshape(-1), root_trans.transpose().reshape(-1)]
+        )
+
     if keys[pygame.K_PERIOD]:
         # Print the current position of the articulated agent, useful for debugging.
         pos = [
@@ -434,6 +480,7 @@ def play_env(env, args, config):
             env,
             not free_cam.is_free_cam_mode,
             agent_to_control,
+            args.use_joint_control,
         )
 
         if not args.no_render and keys[pygame.K_c]:
@@ -612,12 +659,21 @@ def has_pygame():
         action="store_true",
         help="If specified, does not add the inverse kinematics end-effector control.",
     )
+
+    parser.add_argument(
+        "--use-joint-control",
+        action="store_true",
+        default=False,
+        help="Control joints of articulated agent.",
+    )
+
     parser.add_argument(
         "--gfx",
         action="store_true",
         default=False,
         help="Save a GFX replay file.",
     )
+
     parser.add_argument("--load-actions", type=str, default=None)
     parser.add_argument("--cfg", type=str, default=DEFAULT_CFG)
     parser.add_argument(

habitat-lab/habitat/articulated_agents/README.md

@@ -1,7 +1,7 @@
 Robot Design
 ==============================
 
-Habitat supports different kinds of agents that can interact in the environment, including robots or humanoids. These agents are represented as articulated objects, consisting of multiple rigid parts (links) connected by joints to perform rotational or translational motion. This readme file details the design of these articulated agent modules, which robots inherit from. We provide the implementation of different robots under the `robots` folder.
+Habitat supports different kinds of agents that can interact in the environment, including robots or humanoids. These agents are represented as articulated objects, consisting of multiple rigid parts (links) connected by joints to perform rotational or translational motion. This readme file details the design of these articulated agent modules, which robots and humanoids inherit from. We provide the implementation of different robots under the `robots` folder, and the implementation of different humanoids under `humanoids`.
 
 ---
 

habitat-lab/habitat/articulated_agents/articulated_agent_base.py

@@ -25,11 +25,23 @@ def __init__(
         sim: Simulator,
         limit_robo_joints: bool = True,
         fixed_based: bool = True,
+        maintain_link_order=False,
         base_type="mobile",
         sim_obj=None,
         **kwargs,
     ):
-        r"""Constructor"""
+        r"""Constructor
+        :param params: The parameter of the base articulated agent.
+        :param urdf_path: The path to the articulated agent's URDF file.
+        :param sim: The simulator.
+        :param limit_robo_joints: If true, joint limits of articulated agent are always
+            enforced.
+        :param fixed_base: If the articulated agent's base is fixed or not.
+        :param maintain_link_order: Whether to to preserve the order of
+            links parsed from URDF files as link indices. Needed for
+            compatibility with PyBullet.
+        :param sim_obj: Pointer to the simulated object
+        """
         assert base_type in [
             "mobile",
             "leg",
@@ -42,6 +54,7 @@ def __init__(
         self._limit_robo_joints = limit_robo_joints
         self._base_type = base_type
         self.sim_obj = sim_obj
+        self._maintain_link_order = maintain_link_order
 
         # NOTE: the follow members cache static info for improved efficiency over querying the API
         # maps joint ids to motor settings for convenience
@@ -71,7 +84,9 @@ def reconfigure(self) -> None:
         if self.sim_obj is None or not self.sim_obj.is_alive:
             ao_mgr = self._sim.get_articulated_object_manager()
             self.sim_obj = ao_mgr.add_articulated_object_from_urdf(
-                self.urdf_path, fixed_base=self._fixed_base
+                self.urdf_path,
+                fixed_base=self._fixed_base,
+                maintain_link_order=self._maintain_link_order,
             )
         # set correct gains for wheels
         if (

habitat-lab/habitat/articulated_agents/humanoids/README.md

@@ -0,0 +1,35 @@
+Humanoid Design
+==============================
+
+Habitat also supports humanoid avatars. These avatars are represented as articulated objects connected via joints with rotational motion. The current version only includes a kinematic humanoid, which can be controlled by specifying the joint rotations and object transformation.
+
+You can download the humanoid avatar by running:
+
+```
+python -m habitat_sim.utils.datasets_download --uids humanoid_data  --data-path data/
+```
+
+From the home directory.
+
+---
+
+## Humanoid Component Design
+
+1. **Humanoid**
+    - `kinematic_humanoid.py` is built upon `mobile_manipulator.py` and can be controlled by setting the joint and object transforms, via the function `set_joint_transform`.
+
+1. **Mobile Manipulator Parameters**
+    - Parameters such as the camera's transformation, end-effector position, control gain, and more are specified in each robot class (e.g., here, `kinematic_humanoid.py`).
+
+1. **Humanoid Articulated Object**
+
+The humanoid is represented as an articulated object made out of capsules and spheres. It contains 20 links and 19 joints, and is designed to match the morphology of the SMPL skeleton, as referenced [here](https://files.is.tue.mpg.de/black/talks/SMPL-made-simple-FAQs.pdf). Note that the current model does not include the hand or foot joint, hence the 20 links instead of 24, and that the root joint is represented via the object transform, hence the 19 joints.
+
+
+## Testing the Humanoid
+
+You can test a simple demo where the humanoid moves their arms at random using:
+
+```
+python examples/interactive_play.py --never-end --disable-inverse-kinematics --use-joint-control --cfg benchmark/rearrange/play_human.yaml
+```

habitat-lab/habitat/articulated_agents/humanoids/__init__.py

@@ -0,0 +1,13 @@
+#!/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.
+
+from habitat.articulated_agents.humanoids.kinematic_humanoid import (
+    KinematicHumanoid,
+)
+
+__all__ = [
+    "KinematicHumanoid",
+]

habitat-lab/habitat/articulated_agents/humanoids/kinematic_humanoid.py

@@ -0,0 +1,85 @@
+# 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.
+
+from typing import List
+
+import magnum as mn
+import numpy as np
+
+import habitat_sim
+from habitat.articulated_agents.mobile_manipulator import (
+    ArticulatedAgentCameraParams,
+    MobileManipulator,
+    MobileManipulatorParams,
+)
+
+
+class KinematicHumanoid(MobileManipulator):
+    def _get_humanoid_params(self):
+        return MobileManipulatorParams(
+            arm_joints=[],  # For now we do not add arm_joints
+            gripper_joints=[],
+            wheel_joints=None,
+            arm_init_params=None,
+            gripper_init_params=None,
+            gripper_closed_state=np.array([]),
+            gripper_open_state=np.array([]),
+            gripper_state_eps=None,
+            wheel_mtr_pos_gain=None,
+            wheel_mtr_vel_gain=None,
+            wheel_mtr_max_impulse=None,
+            ee_offset=[mn.Vector3(), mn.Vector3()],
+            ee_links=[15, 9],
+            ee_constraint=np.zeros((2, 2, 3)),
+            cameras={
+                "robot_head": ArticulatedAgentCameraParams(
+                    cam_offset_pos=mn.Vector3(0.0, 0.5, 0.25),
+                    cam_look_at_pos=mn.Vector3(0.0, 0.5, 0.75),
+                    attached_link_id=-1,
+                ),
+                "robot_third": ArticulatedAgentCameraParams(
+                    cam_offset_pos=mn.Vector3(-1.2, 2.0, -1.2),
+                    cam_look_at_pos=mn.Vector3(1, 0.0, 0.75),
+                    attached_link_id=-1,
+                ),
+            },
+            arm_mtr_pos_gain=0.3,
+            arm_mtr_vel_gain=0.3,
+            arm_mtr_max_impulse=10.0,
+            base_offset=mn.Vector3(0, -0.9, 0),
+            base_link_names={
+                "base_link",
+            },
+            ee_count=2,
+        )
+
+    def __init__(
+        self, urdf_path, sim, limit_robo_joints=False, fixed_base=False
+    ):
+        super().__init__(
+            self._get_humanoid_params(),
+            urdf_path,
+            sim,
+            limit_robo_joints,
+            fixed_base,
+            maintain_link_order=True,
+        )
+
+    def reconfigure(self) -> None:
+        """Instantiates the human in the scene. Loads the URDF, sets initial state of parameters, joints, motors, etc..."""
+        super().reconfigure()
+        self.sim_obj.motion_type = habitat_sim.physics.MotionType.KINEMATIC
+
+    def set_joint_transform(
+        self, joint_list: List[float], transform: mn.Matrix4
+    ) -> None:
+        """Sets the joints and base transform of the humanoid"""
+        # TODO: should this go into articulated agent?
+        self.sim_obj.joint_positions = joint_list
+        self.sim_obj.transformation = transform
+
+    def get_joint_transform(self):
+        """Returns the joints and base transform of the humanoid"""
+        # TODO: should this go into articulated agent?
+        return self.sim_obj.joint_positions, self.sim_obj.transformation

habitat-lab/habitat/articulated_agents/manipulator.py

@@ -27,6 +27,7 @@ def __init__(
         limit_robo_joints: bool = True,
         fixed_based: bool = True,
         sim_obj=None,
+        maintain_link_order=False,
         **kwargs,
     ):
         r"""Constructor"""
@@ -38,6 +39,7 @@ def __init__(
         self._limit_robo_joints = limit_robo_joints
         self._fixed_base = fixed_based
         self.sim_obj = sim_obj
+        self._maintain_link_order = maintain_link_order
 
         # Adapt Manipulator params to support multiple end effector indices
         # NOTE: the follow members cache static info for improved efficiency over querying the API
@@ -78,7 +80,9 @@ def reconfigure(self) -> None:
         if self.sim_obj is None or not self.sim_obj.is_alive:
             ao_mgr = self._sim.get_articulated_object_manager()
             self.sim_obj = ao_mgr.add_articulated_object_from_urdf(
-                self.urdf_path, fixed_base=self._fixed_base
+                self.urdf_path,
+                fixed_base=self._fixed_base,
+                maintain_link_order=self._maintain_link_order,
             )
         if self._limit_robo_joints:
             # automatic joint limit clamping after each call to sim.step_physics()
@@ -177,6 +181,7 @@ def update(self) -> None:
                     sens_obj.node.transformation = (
                         orthonormalize_rotation_shear(cam_transform)
                     )
+
         if self._fix_joint_values is not None:
             self.arm_joint_pos = self._fix_joint_values
 

habitat-lab/habitat/articulated_agents/mobile_manipulator.py

@@ -92,9 +92,9 @@ class MobileManipulatorParams:
     arm_mtr_vel_gain: float
     arm_mtr_max_impulse: float
 
-    wheel_mtr_pos_gain: float
-    wheel_mtr_vel_gain: float
-    wheel_mtr_max_impulse: float
+    wheel_mtr_pos_gain: Optional[float]
+    wheel_mtr_vel_gain: Optional[float]
+    wheel_mtr_max_impulse: Optional[float]
 
     base_offset: mn.Vector3
     base_link_names: Set[str]
@@ -112,15 +112,19 @@ def __init__(
         sim: Simulator,
         limit_robo_joints: bool = True,
         fixed_base: bool = True,
+        maintain_link_order: bool = False,
         base_type="mobile",
     ):
         r"""Constructor
-        :param params: The parameter of the manipulator robot.
-        :param urdf_path: The path to the robot's URDF file.
+        :param params: The parameter of the manipulator articulated agent.
+        :param urdf_path: The path to the agent's URDF file.
         :param sim: The simulator.
-        :param limit_robo_joints: If true, joint limits of robot are always
+        :param limit_robo_joints: If true, joint limits of agent are always
             enforced.
         :param fixed_base: If the robot's base is fixed or not.
+        :param maintain_link_order: Whether to to preserve the order of
+            links parsed from URDF files as link indices. Needed for
+            compatibility with PyBullet.
         :param base_type: The base type
         """
         # instantiate a manipulator
@@ -141,6 +145,7 @@ def __init__(
             fixed_based=fixed_base,
             sim_obj=self.sim_obj,
             base_type=base_type,
+            maintain_link_order=maintain_link_order,
         )
 
     def reconfigure(self) -> None: