diff --git a/examples/interactive_play.py b/examples/interactive_play.py index 10ec19fb31..4b18a26984 100644 --- a/examples/interactive_play.py +++ b/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, + control_humanoid, ): 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 control_humanoid: + 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,45 @@ def get_input_vel_ctlr( logger.info("[play.py]: Snapping") magic_grasp = 1 + if control_humanoid: + # Add random noise to human arms but keep global transform + ( + joint_trans, + root_trans, + ) = env._sim.articulated_agent.get_joint_transform() + # Divide joint_trans by 4 since joint_trans has flattened quaternions + # and the dimension of each quaternion is 4 + num_joints = len(joint_trans) // 4 + root_trans = np.array(root_trans) + index_arms_start = 10 + joint_trans_quat = [ + mn.Quaternion( + mn.Vector3(joint_trans[(4 * index) : (4 * index + 3)]), + joint_trans[4 * index + 3], + ) + for index in range(num_joints) + ] + rotated_joints_quat = [] + for index, joint_quat in enumerate(joint_trans_quat): + random_vec = np.random.rand(3) + # We allow for maximum 10 angles per step + random_angle = np.random.rand() * 10 + rotation_quat = mn.Quaternion.rotation( + mn.Rad(random_angle), mn.Vector3(random_vec).normalized() + ) + if index > index_arms_start: + joint_quat *= rotation_quat + rotated_joints_quat.append(joint_quat) + joint_trans = np.concatenate( + [ + np.array(list(quat.vector) + [quat.scalar]) + for quat in rotated_joints_quat + ] + ) + base_action = np.concatenate( + [joint_trans.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 +483,7 @@ def play_env(env, args, config): env, not free_cam.is_free_cam_mode, agent_to_control, + args.control_humanoid, ) if not args.no_render and keys[pygame.K_c]: @@ -612,6 +662,14 @@ def has_pygame(): action="store_true", help="If specified, does not add the inverse kinematics end-effector control.", ) + + parser.add_argument( + "--control-humanoid", + action="store_true", + default=False, + help="Control humanoid agent.", + ) + parser.add_argument( "--gfx", action="store_true", @@ -671,6 +729,9 @@ def has_pygame(): if args.never_end: env_config.max_episode_steps = 0 + if args.control_humanoid: + args.disable_inverse_kinematics = True + if not args.disable_inverse_kinematics: if "arm_action" not in task_config.actions: raise ValueError( diff --git a/habitat-lab/habitat/articulated_agents/README.md b/habitat-lab/habitat/articulated_agents/README.md index fb0383c0fe..6c2026cfd8 100644 --- a/habitat-lab/habitat/articulated_agents/README.md +++ b/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`. --- diff --git a/habitat-lab/habitat/articulated_agents/articulated_agent_base.py b/habitat-lab/habitat/articulated_agents/articulated_agent_base.py index dba02ef040..03f58e5e13 100644 --- a/habitat-lab/habitat/articulated_agents/articulated_agent_base.py +++ b/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 ( diff --git a/habitat-lab/habitat/articulated_agents/humanoids/README.md b/habitat-lab/habitat/articulated_agents/humanoids/README.md new file mode 100644 index 0000000000..3d2ac55e49 --- /dev/null +++ b/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 --control-humanoid --cfg benchmark/rearrange/play_human.yaml +``` diff --git a/habitat-lab/habitat/articulated_agents/humanoids/__init__.py b/habitat-lab/habitat/articulated_agents/humanoids/__init__.py new file mode 100644 index 0000000000..a8b71f0350 --- /dev/null +++ b/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", +] diff --git a/habitat-lab/habitat/articulated_agents/humanoids/kinematic_humanoid.py b/habitat-lab/habitat/articulated_agents/humanoids/kinematic_humanoid.py new file mode 100644 index 0000000000..f227ea535e --- /dev/null +++ b/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 diff --git a/habitat-lab/habitat/articulated_agents/manipulator.py b/habitat-lab/habitat/articulated_agents/manipulator.py index 63bed7ada3..0ddd327f87 100644 --- a/habitat-lab/habitat/articulated_agents/manipulator.py +++ b/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 diff --git a/habitat-lab/habitat/articulated_agents/mobile_manipulator.py b/habitat-lab/habitat/articulated_agents/mobile_manipulator.py index 2f7c9104fc..fa77f8a2fe 100644 --- a/habitat-lab/habitat/articulated_agents/mobile_manipulator.py +++ b/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: diff --git a/habitat-lab/habitat/config/benchmark/rearrange/play_human.yaml b/habitat-lab/habitat/config/benchmark/rearrange/play_human.yaml new file mode 100644 index 0000000000..b3e5de0238 --- /dev/null +++ b/habitat-lab/habitat/config/benchmark/rearrange/play_human.yaml @@ -0,0 +1,44 @@ +# @package _global_ + +defaults: + - /habitat: habitat_config_base + - /habitat/simulator/agents@habitat.simulator.agents.main_agent: rgbd_head_rgbd_arm_agent + - /habitat/task/rearrange: play_human + - /habitat/dataset/rearrangement: replica_cad + - _self_ + +# Config for empty task to explore the scene. +habitat: + environment: + max_episode_steps: 0 + task: + actions: + humanjoint_action: + type: "HumanoidJointAction" + simulator: + type: RearrangeSim-v0 + seed: 100 + additional_object_paths: + - "data/objects/ycb/configs/" + agents: + main_agent: + radius: 0.3 + robot_urdf: 'data/humanoid_data/amass_male.urdf' + robot_type: 'KinematicHumanoid' + sim_sensors: + head_rgb_sensor: + height: 128 + width: 128 + head_depth_sensor: + height: 128 + width: 128 + arm_depth_sensor: + height: 128 + width: 128 + arm_rgb_sensor: + height: 128 + width: 128 + habitat_sim_v0: + enable_physics: True + dataset: + data_path: data/datasets/replica_cad/rearrange/v1/{split}/all_receptacles_10k_1k.json.gz diff --git a/habitat-lab/habitat/config/habitat/task/rearrange/play_human.yaml b/habitat-lab/habitat/config/habitat/task/rearrange/play_human.yaml new file mode 100644 index 0000000000..fbc3b589b1 --- /dev/null +++ b/habitat-lab/habitat/config/habitat/task/rearrange/play_human.yaml @@ -0,0 +1,25 @@ +# @package habitat.task + +defaults: + - /habitat/task: task_config_base + - /habitat/task/measurements: + - robot_force + - /habitat/task/lab_sensors: + - joint_sensor + - _self_ + +# Config for empty task to explore the scene. +type: RearrangeEmptyTask-v0 +count_obj_collisions: True +desired_resting_position: [0.5, 0.0, 1.0] + +# Reach task config +render_target: True +ee_sample_factor: 0.8 + +# In radians +base_angle_noise: 0.0 +base_noise: 0.0 +constraint_violation_ends_episode: False + +force_regenerate: True diff --git a/habitat-lab/habitat/tasks/rearrange/__init__.py b/habitat-lab/habitat/tasks/rearrange/__init__.py index 367b97d03e..dad54e482b 100644 --- a/habitat-lab/habitat/tasks/rearrange/__init__.py +++ b/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") diff --git a/habitat-lab/habitat/tasks/rearrange/actions/actions.py b/habitat-lab/habitat/tasks/rearrange/actions/actions.py index ffe7bb986a..21826a9228 100644 --- a/habitat-lab/habitat/tasks/rearrange/actions/actions.py +++ b/habitat-lab/habitat/tasks/rearrange/actions/actions.py @@ -467,3 +467,54 @@ 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, human_joints_trans, **kwargs): + r""" + Updates the joint rotations and root transformation of the humanoid. + :param human_joint_trans: Array of size (num_joints*4)+16. The last 16 + dimensions define the 4x4 root transformation matrix, the first elements + correspond to a flattened list of quaternions for each joint. When the array is all 0 + it keeps the previous joint rotation and transform. + """ + new_joints = human_joints_trans[:-16] + new_pos_transform = human_joints_trans[-16:] + + # When the array is all 0, this indicates we are not setting + # the human joint + 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) diff --git a/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py b/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py index da04e3d63a..c4e195155a 100644 --- a/habitat-lab/habitat/tasks/rearrange/articulated_agent_manager.py +++ b/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