g1.py

# SPDX-FileCopyrightText: Copyright (c) 2025-2026 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
# SPDX-License-Identifier: Apache-2.0

from protomotions.robot_configs.base import (
    RobotConfig,
    RobotAssetConfig,
    ControlConfig,
    ControlType,
    SimulatorParams,
)
from protomotions.simulator.isaacgym.config import (
    IsaacGymSimParams,
    IsaacGymPhysXParams,
)
from protomotions.simulator.isaaclab.config import (
    IsaacLabSimParams,
    IsaacLabPhysXParams,
)
from protomotions.simulator.genesis.config import GenesisSimParams
from protomotions.simulator.newton.config import NewtonSimParams
from protomotions.components.pose_lib import ControlInfo
from typing import List, Dict, Tuple
from dataclasses import dataclass, field


# Parameters from BeyondMimic (https://github.com/HybridRobotics/whole_body_tracking/blob/main/source/whole_body_tracking/whole_body_tracking/robots/g1.py)
ARMATURE_5020 = 0.003609725
ARMATURE_7520_14 = 0.010177520
ARMATURE_7520_22 = 0.025101925
ARMATURE_4010 = 0.00425

NATURAL_FREQ = 10 * 2.0 * 3.1415926535  # 10Hz
DAMPING_RATIO = 2.0

STIFFNESS_5020 = ARMATURE_5020 * NATURAL_FREQ**2
STIFFNESS_7520_14 = ARMATURE_7520_14 * NATURAL_FREQ**2
STIFFNESS_7520_22 = ARMATURE_7520_22 * NATURAL_FREQ**2
STIFFNESS_4010 = ARMATURE_4010 * NATURAL_FREQ**2

DAMPING_5020 = 2.0 * DAMPING_RATIO * ARMATURE_5020 * NATURAL_FREQ
DAMPING_7520_14 = 2.0 * DAMPING_RATIO * ARMATURE_7520_14 * NATURAL_FREQ
DAMPING_7520_22 = 2.0 * DAMPING_RATIO * ARMATURE_7520_22 * NATURAL_FREQ
DAMPING_4010 = 2.0 * DAMPING_RATIO * ARMATURE_4010 * NATURAL_FREQ

# Default standing pose (radians), from BeyondMimic.
# Regex patterns resolved against DOF names in RobotConfig.__post_init__.
DEFAULT_JOINT_POS = {
    ".*_hip_pitch_joint": -0.312,
    ".*_knee_joint": 0.669,
    ".*_ankle_pitch_joint": -0.363,
    ".*_elbow_joint": 0.6,
    "left_shoulder_roll_joint": 0.2,
    "left_shoulder_pitch_joint": 0.2,
    "right_shoulder_roll_joint": -0.2,
    "right_shoulder_pitch_joint": 0.2,
}


@dataclass
class G1RobotConfig(RobotConfig):
    semantic_forward_axis_xy: Tuple[float, float] = (1.0, 0.0)

    common_naming_to_robot_body_names: Dict[str, List[str]] = field(
        default_factory=lambda: {
            "all_left_foot_bodies": ["left_ankle_roll_link"],
            "all_right_foot_bodies": ["right_ankle_roll_link"],
            "all_left_hand_bodies": ["left_rubber_hand"],
            "all_right_hand_bodies": ["right_rubber_hand"],
            "head_body_name": ["head"],
            "torso_body_name": ["torso_link"],
        }
    )

    trackable_bodies_subset: List[str] = field(
        default_factory=lambda: [
            "torso_link",
            "head",
            "right_ankle_roll_link",
            "left_ankle_roll_link",
            "left_rubber_hand",
            "right_rubber_hand",
        ]
    )

    default_root_height: float = 0.8
    default_dof_pos: Dict[str, float] = field(default_factory=lambda: DEFAULT_JOINT_POS)
    anchor_body_name: str = "torso_link"

    asset: RobotAssetConfig = field(
        default_factory=lambda: RobotAssetConfig(
            asset_file_name="mjcf/g1_holo_compat.xml",
            replace_cylinder_with_capsule=True,
            thickness=0.01,
            max_angular_velocity=1000.0,
            max_linear_velocity=1000.0,
            density=0.001,
            angular_damping=0.0,
            linear_damping=0.0,
        )
    )

    control: ControlConfig = field(
        default_factory=lambda: ControlConfig(
            control_type=ControlType.BUILT_IN_PD,
            override_control_info={
                ".*_hip_(pitch|yaw)_joint": ControlInfo(
                    stiffness=STIFFNESS_7520_14,
                    damping=DAMPING_7520_14,
                    effort_limit=88,
                    velocity_limit=32,
                    armature=ARMATURE_7520_14,
                ),
                ".*_hip_roll_joint": ControlInfo(
                    stiffness=STIFFNESS_7520_22,
                    damping=DAMPING_7520_22,
                    effort_limit=139,
                    velocity_limit=20,
                    armature=ARMATURE_7520_22,
                ),
                ".*_knee_joint": ControlInfo(
                    stiffness=STIFFNESS_7520_22,
                    damping=DAMPING_7520_22,
                    effort_limit=139,
                    velocity_limit=20,
                    armature=ARMATURE_7520_22,
                ),
                ".*_ankle_.*": ControlInfo(
                    stiffness=2 * STIFFNESS_5020,
                    damping=2 * DAMPING_5020,
                    effort_limit=50,
                    velocity_limit=37,
                    armature=2 * ARMATURE_5020,
                ),
                "waist_yaw_joint": ControlInfo(
                    stiffness=STIFFNESS_7520_14,
                    damping=DAMPING_7520_14,
                    effort_limit=88,
                    velocity_limit=32,
                    armature=ARMATURE_7520_14,
                ),
                "waist_(roll|pitch)_joint": ControlInfo(
                    stiffness=2.0 * STIFFNESS_5020,
                    damping=2.0 * DAMPING_5020,
                    effort_limit=50,
                    velocity_limit=37,
                    armature=2.0 * ARMATURE_5020,
                ),
                ".*_(shoulder|elbow)_.*": ControlInfo(
                    stiffness=STIFFNESS_5020,
                    damping=DAMPING_5020,
                    effort_limit=25,
                    velocity_limit=37,
                    armature=ARMATURE_5020,
                ),
                ".*_wrist_roll_joint": ControlInfo(
                    stiffness=STIFFNESS_5020,
                    damping=DAMPING_5020,
                    effort_limit=25,
                    velocity_limit=37,
                    armature=ARMATURE_5020,
                ),
                ".*_wrist_pitch_joint": ControlInfo(
                    stiffness=STIFFNESS_4010,
                    damping=DAMPING_4010,
                    effort_limit=5,
                    velocity_limit=22,
                    armature=ARMATURE_4010,
                ),
                ".*_wrist_yaw_joint": ControlInfo(
                    stiffness=STIFFNESS_4010,
                    damping=DAMPING_4010,
                    effort_limit=5,
                    velocity_limit=22,
                    armature=ARMATURE_4010,
                ),
            },
        )
    )

    simulation_params: SimulatorParams = field(
        default_factory=lambda: SimulatorParams(
            isaacgym=IsaacGymSimParams(
                fps=100,
                decimation=2,
                substeps=2,
                physx=IsaacGymPhysXParams(
                    num_position_iterations=8,
                    num_velocity_iterations=4,
                    max_depenetration_velocity=1,
                ),
            ),
            isaaclab=IsaacLabSimParams(
                fps=200,
                decimation=4,
                physx=IsaacLabPhysXParams(
                    num_position_iterations=8,
                    num_velocity_iterations=4,
                    max_depenetration_velocity=1,
                ),
            ),
            genesis=GenesisSimParams(
                fps=100,
                decimation=2,
                substeps=2,
            ),
            newton=NewtonSimParams(
                fps=200,
                decimation=4,
            ),
        )
    )