Humanoids pr

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 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

@@ -27,6 +27,7 @@ def __init__(
         fixed_based: bool = True,
         base_type="mobile",
         sim_obj=None,
+        maintain_link_order=False,
         **kwargs,
     ):
         r"""Constructor"""
@@ -42,6 +43,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 +73,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,18 @@
+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.
+
+---
+
+## 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.

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,80 @@
+# 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.physics as phy
+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 = phy.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

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,6 +112,7 @@ def __init__(
         sim: Simulator,
         limit_robo_joints: bool = True,
         fixed_base: bool = True,
+        maintain_link_order: bool = True,
         base_type="mobile",
     ):
         r"""Constructor
@@ -141,6 +142,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:

habitat-lab/habitat/tasks/rearrange/__init__.py

@@ -49,3 +49,5 @@ def _try_register_rearrange_task():
         HabitatSimActions.extend_action_space("empty")
     if not HabitatSimActions.has_action("rearrange_stop"):
         HabitatSimActions.extend_action_space("rearrange_stop")
+    if not HabitatSimActions.has_action("changejoint_action"):
+        HabitatSimActions.extend_action_space("changejoint_action")

habitat-lab/habitat/tasks/rearrange/actions/actions.py

@@ -467,3 +467,45 @@ def step(self, ee_pos, **kwargs):
             self._sim.viz_ids["ee_target"] = self._sim.visualize_position(
                 global_pos, self._sim.viz_ids["ee_target"]
             )
+
+
+@registry.register_task_action
+class HumanoidJointAction(RobotAction):
+    def __init__(self, *args, sim: RearrangeSim, **kwargs):
+        super().__init__(*args, sim=sim, **kwargs)
+        self._sim: RearrangeSim = sim
+        self.num_joints = 19
+
+    def reset(self, *args, **kwargs):
+        super().reset()
+
+    @property
+    def action_space(self):
+        num_joints = self.num_joints
+        num_dim_transform = 16
+        # The action space is the number of joints plus 16 for a 4x4 transformtion matrix for the base
+        return spaces.Dict(
+            {
+                "human_joints_trans": spaces.Box(
+                    shape=(4 * (num_joints + num_dim_transform),),
+                    low=-1,
+                    high=1,
+                    dtype=np.float32,
+                )
+            }
+        )
+
+    def step(self, **kwargs):
+        new_pos_transform = kwargs["human_joints_trans"]
+        new_joints = new_pos_transform[:-16]
+        new_pos_transform = new_pos_transform[-16:]
+        if np.array(new_pos_transform).sum() != 0:
+            vecs = [
+                mn.Vector4(new_pos_transform[i * 4 : (i + 1) * 4])
+                for i in range(4)
+            ]
+            new_transform = mn.Matrix4(*vecs)
+            self.cur_articulated_agent.set_joint_transform(
+                new_joints, new_transform
+            )
+        return self._sim.step(HabitatSimActions.changejoint_action)

habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py

@@ -8,6 +8,9 @@
 import magnum as mn
 import numpy as np
 
+from habitat.articulated_agents.humanoids.kinematic_humanoid import (
+    KinematicHumanoid,
+)
 from habitat.articulated_agents.mobile_manipulator import MobileManipulator
 
 # flake8: noqa
@@ -120,7 +123,6 @@ def post_obj_load_reconfigure(self):
         """
         Called at the end of the simulator reconfigure method. Used to set the starting configurations of the robots if specified in the task config.
         """
-
         for agent_data in self._all_agent_data:
             agent_data.articulated_agent.params.arm_init_params = (
                 agent_data.start_js