[Feature] add G1 robot (#405)

* add G1 robot

* add `UnitreeG1Simplified`; update env `UnitreeG1Stand-v1`

* update url of `unitree_g1` model

* rename `unitree_g1_simplified` to `unitree_g1_simplified_legs`

examples/baselines/ppo/README.md

@@ -89,6 +89,10 @@ python ppo.py --env_id="UnitreeH1Stand-v1" \
   --num_envs=1024 --update_epochs=8 --num_minibatches=32 \
   --total_timesteps=100_000_000 --num-steps=100 --num-eval-steps=1000 \
   --gamma=0.99 --gae_lambda=0.95
+python ppo.py --env_id="UnitreeG1Stand-v1" \
+  --num_envs=1024 --update_epochs=8 --num_minibatches=32 \
+  --total_timesteps=100_000_000 --num-steps=100 --num-eval-steps=1000 \
+  --gamma=0.99 --gae_lambda=0.95
 
 python ppo.py --env_id="OpenCabinetDrawer-v1" \
   --num_envs=1024 --update_epochs=8 --num_minibatches=32 \

mani_skill/agents/robots/__init__.py

@@ -9,6 +9,7 @@
 from .trifingerpro import TriFingerPro
 from .unitree_go import *
 from .unitree_h1 import *
+from .unitree_g1 import *
 from .ur_e import UR10e
 from .widowx import *
 from .xarm import XArm7Ability

mani_skill/agents/robots/unitree_g1/__init__.py

@@ -0,0 +1 @@
+from .g1 import UnitreeG1, UnitreeG1Simplified

mani_skill/agents/robots/unitree_g1/g1.py

@@ -0,0 +1,118 @@
+import numpy as np
+import sapien
+
+from mani_skill import ASSET_DIR
+from mani_skill.agents.base_agent import BaseAgent, Keyframe
+from mani_skill.agents.controllers import *
+from mani_skill.agents.registration import register_agent
+from mani_skill.sensors.camera import CameraConfig
+
+
+@register_agent()
+class UnitreeG1(BaseAgent):
+    uid = "unitree_g1"
+    urdf_path = f"{ASSET_DIR}/robots/unitree_g1/g1.urdf"
+    urdf_config = dict()
+    fix_root_link = False
+    load_multiple_collisions = True
+
+    keyframes = dict(
+        standing=Keyframe(
+            pose=sapien.Pose(p=[0, 0, 0.755]),
+            qpos=np.array(
+                [0.0] * 37
+            )
+            * 1,
+        )
+    )
+
+    body_joints = [
+        "left_hip_pitch_joint",
+        "right_hip_pitch_joint",
+        "torso_joint",
+        "left_hip_roll_joint",
+        "right_hip_roll_joint",
+        "left_shoulder_pitch_joint",
+        "right_shoulder_pitch_joint",
+        "left_hip_yaw_joint",
+        "right_hip_yaw_joint",
+        "left_shoulder_roll_joint",
+        "right_shoulder_roll_joint",
+        "left_knee_joint",
+        "right_knee_joint",
+        "left_shoulder_yaw_joint",
+        "right_shoulder_yaw_joint",
+        "left_ankle_pitch_joint",
+        "right_ankle_pitch_joint",
+        "left_elbow_pitch_joint",
+        "right_elbow_pitch_joint",
+        "left_ankle_roll_joint",
+        "right_ankle_roll_joint",
+        "left_elbow_roll_joint",
+        "right_elbow_roll_joint",
+        "left_zero_joint",
+        "left_three_joint",
+        "left_five_joint",
+        "right_zero_joint",
+        "right_three_joint",
+        "right_five_joint",
+        "left_one_joint",
+        "left_four_joint",
+        "left_six_joint",
+        "right_one_joint",
+        "right_four_joint",
+        "right_six_joint",
+        "left_two_joint",
+        "right_two_joint",
+    ]
+    body_stiffness = 50
+    body_damping = 1
+    body_force_limit = 100
+
+    @property
+    def _controller_configs(self):
+        body_pd_joint_pos = PDJointPosControllerConfig(
+            self.body_joints,
+            lower=None,
+            upper=None,
+            stiffness=self.body_stiffness,
+            damping=self.body_damping,
+            force_limit=self.body_force_limit,
+            normalize_action=False,
+        )
+        body_pd_joint_delta_pos = PDJointPosControllerConfig(
+            self.body_joints,
+            lower=-0.2,
+            upper=0.2,
+            stiffness=self.body_stiffness,
+            damping=self.body_damping,
+            force_limit=self.body_force_limit,
+            use_delta=True,
+        )
+        # note we must add balance_passive_force=False otherwise gravity will be disabled for the robot itself
+        # balance_passive_force=True is only useful for fixed robots
+        return dict(
+            pd_joint_pos=dict(body=body_pd_joint_pos, balance_passive_force=False),
+            pd_joint_delta_pos=dict(
+                body=body_pd_joint_delta_pos, balance_passive_force=False
+            ),
+        )
+
+    @property
+    def _sensor_configs(self):
+        return []
+
+    def is_standing(self):
+        """Checks if G1 is standing with a simple heuristic of checking if the torso is at a minimum height"""
+        # TODO add check for rotation of torso? so that robot can't fling itself off the floor and rotate everywhere?
+        return (self.robot.pose.p[:, 2] > 0.5) & (self.robot.pose.p[:, 2] < 1.0)
+
+    def is_fallen(self):
+        """Checks if G1 has fallen on the ground. Effectively checks if the torso is too low"""
+        return self.robot.pose.p[:, 2] < 0.3
+
+
+@register_agent()
+class UnitreeG1Simplified(UnitreeG1):
+    uid = "unitree_g1_simplified_legs"
+    urdf_path = f"{ASSET_DIR}/robots/unitree_g1/g1_simplified_legs.urdf"

mani_skill/envs/tasks/humanoid/humanoid_stand.py

@@ -4,7 +4,7 @@
 import sapien
 import torch
 
-from mani_skill.agents.robots import UnitreeH1, UnitreeH1Simplified
+from mani_skill.agents.robots import UnitreeH1Simplified, UnitreeG1Simplified
 from mani_skill.envs.sapien_env import BaseEnv
 from mani_skill.sensors.camera import CameraConfig
 from mani_skill.utils import common, sapien_utils
@@ -84,6 +84,41 @@ def _default_human_render_camera_configs(self):
         pose = sapien_utils.look_at([1.0, 1.0, 2.5], [0.0, 0.0, 0.75])
         return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
 
+    def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
+        with torch.device(self.device):
+            b = len(env_idx)
+            standing_keyframe = self.agent.keyframes["standing"]
+            random_qpos = (
+                torch.randn(
+                    size=(b, self.agent.robot.dof[0]), dtype=torch.float) * 0.05
+            )
+            random_qpos += common.to_tensor(standing_keyframe.qpos,
+                                            device=self.device)
+            self.agent.robot.set_qpos(random_qpos)
+            self.agent.robot.set_pose(sapien.Pose(p=[0, 0, 0.975]))
+
+
+@register_env("UnitreeG1Stand-v1", max_episode_steps=1000)
+class UnitreeG1StandEnv(HumanoidStandEnv):
+    SUPPORTED_ROBOTS = ["unitree_g1_simplified_legs"]
+    agent: Union[UnitreeG1Simplified]
+
+    def __init__(self, *args, robot_uids="unitree_g1_simplified_legs", **kwargs):
+        super().__init__(*args, robot_uids=robot_uids, **kwargs)
+
+    @property
+    def _default_sim_config(self):
+        return SimConfig(
+            gpu_memory_cfg=GPUMemoryConfig(
+                max_rigid_contact_count=2**22, max_rigid_patch_count=2**21
+            )
+        )
+
+    @property
+    def _default_human_render_camera_configs(self):
+        pose = sapien_utils.look_at([1.0, 1.0, 2.0], [0.0, 0.0, 0.75])
+        return CameraConfig("render_camera", pose, 512, 512, 1, 0.01, 100)
+
     def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
         with torch.device(self.device):
             b = len(env_idx)
@@ -93,4 +128,4 @@ def _initialize_episode(self, env_idx: torch.Tensor, options: dict):
             )
             random_qpos += common.to_tensor(standing_keyframe.qpos, device=self.device)
             self.agent.robot.set_qpos(random_qpos)
-            self.agent.robot.set_pose(sapien.Pose(p=[0, 0, 0.975]))
+            self.agent.robot.set_pose(sapien.Pose(p=[0, 0, 0.755]))

mani_skill/utils/download_asset.py

@@ -186,6 +186,11 @@ def initialize_extra_sources():
         url="https://github.com/haosulab/ManiSkill-UnitreeH1/archive/refs/tags/v0.1.0.zip",
         target_path="robots/unitree_h1",
     )
+    DATA_SOURCES["unitree_g1"] = DataSource(
+        source_type="robot",
+        url="https://github.com/haosulab/ManiSkill-UnitreeG1/archive/refs/tags/v0.1.0.zip",
+        target_path="robots/unitree_g1",
+    )
     DATA_SOURCES["unitree_go2"] = DataSource(
         source_type="robot",
         url="https://github.com/haosulab/ManiSkill-UnitreeGo2/archive/refs/tags/v0.1.0.zip",