# SPDX-FileCopyrightText: Copyright (c) 2025-2026 The ProtoMotions Developers # SPDX-License-Identifier: Apache-2.0 # # Licensed under the Apache License, Version 2.0 (the "License"); # you may not use this file except in compliance with the License. # You may obtain a copy of the License at # # http://www.apache.org/licenses/LICENSE-2.0 # # Unless required by applicable law or agreed to in writing, software # distributed under the License is distributed on an "AS IS" BASIS, # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. # See the License for the specific language governing permissions and # limitations under the License. # 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 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): 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", usd_asset_file_name="usd/g1_holo_compat/g1_holo_compat.usda", usd_bodies_root_prim_path="/World/envs/env_.*/Robot/pelvis/", 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, ), ) )