From 478a16114285c3819e29df5a68399873bbc2bfdc Mon Sep 17 00:00:00 2001 From: ruziniuuuuu Date: Sat, 12 Sep 2026 15:56:15 +0800 Subject: [PATCH] feat(galbot): apply Sim2Real actuator gains and gravity compensation MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Replace the pass-through USD actuator config with system-identified gains from the galbot-one-golf-sim2real-v1 profile (synthnova/src/synthnova/robots/cfg/galbot_one_golf.toml): - Arm shoulder (j1–j2): stiffness 3500/3400 N·m/rad, effort ±180 N·m - Arm elbow (j3–j4): stiffness 1145/1143 N·m/rad, effort ±90 N·m - Arm wrist (j5–j7): stiffness 284–286 N·m/rad, effort ±30 N·m - Leg joints : per-joint stiffness/damping, effort 70–433 N·m - Head joints : stiffness 80 N·m/rad, effort ±4 N·m - Grippers : stiffness 77 N·m/rad, effort ±1.5 N·m Left/right arm values are symmetrised (averaged) and rounded to integers. Velocity limits are set from the profile for all body joints. Gravity compensation is enabled via disable_gravity=True on the articulation's rigid bodies, matching the real robot's controller-side compensation and consistent with RoboLab's franka_high_pd convention. Two new tests are added to test_galbot_configs.py: - test_gravity_compensation_is_enabled - test_actuator_gains_match_sim2real_profile Co-Authored-By: Claude Opus 5 --- robolab/robots/galbot_golf.py | 121 ++++++++++++++++++++++++++++------ tests/test_galbot_configs.py | 45 +++++++++++++ 2 files changed, 146 insertions(+), 20 deletions(-) diff --git a/robolab/robots/galbot_golf.py b/robolab/robots/galbot_golf.py index e9747d82..b7bb49e8 100644 --- a/robolab/robots/galbot_golf.py +++ b/robolab/robots/galbot_golf.py @@ -134,7 +134,17 @@ def _galbot_golf_robot_cfg( init_joint_pos: dict[str, float] | None = None, init_pos: tuple[float, float, float] = (0.0, 0.0, 0.0), ) -> ArticulationCfg: - """Build a Golf articulation config while preserving the received USD physics properties.""" + """Build a Golf articulation config with Sim2Real gains and controller gravity compensation. + + Stiffness, damping, and effort limits are sourced from the + ``galbot-one-golf-sim2real-v1`` system-identification profile + (synthnova/src/synthnova/robots/cfg/galbot_one_golf.toml). + Left/right arm gains are symmetrised and rounded to integers. + + Gravity compensation is realised by disabling per-link gravity in the + physics engine (``disable_gravity=True``), matching the behaviour of the + real robot's controller-side compensation. + """ return ArticulationCfg( prim_path="{ENV_REGEX_NS}/robot", spawn=sim_utils.UsdFileCfg( @@ -142,6 +152,10 @@ def _galbot_golf_robot_cfg( variants=GALBOT_GOLF_USD_VARIANTS, activate_contact_sensors=True, rigid_props=sim_utils.RigidBodyPropertiesCfg( + # Simulate controller-side gravity compensation: the real robot + # compensates gravity in its low-level controller, so the + # joint-position PD loops only see tracking error, not gravity load. + disable_gravity=True, max_depenetration_velocity=5.0, ), articulation_props=sim_utils.ArticulationRootPropertiesCfg( @@ -167,35 +181,102 @@ def _galbot_golf_robot_cfg( ), soft_joint_pos_limit_factor=1.0, actuators={ - # Preserve all drive properties authored in the supplied PhysX USD. + # --- Leg linkage (Sim2Real profile, per-joint) --- + # The five leg joints span three torque tiers; all values are + # taken directly from the system-identification profile. "legs": ImplicitActuatorCfg( joint_names_expr=["leg_joint.*"], - effort_limit_sim=None, - velocity_limit_sim=None, - stiffness=None, - damping=None, + effort_limit_sim={ + "leg_joint1": 433, + "leg_joint2": 433, + "leg_joint3": 204, + "leg_joint4": 70, + "leg_joint5": 70, + }, + velocity_limit_sim={ + "leg_joint1": 2.094395, + "leg_joint2": 2.094395, + "leg_joint3": 3.141593, + "leg_joint4": 3.141593, + "leg_joint5": 3.141593, + }, + stiffness={ + "leg_joint1": 8660, + "leg_joint2": 8660, + "leg_joint3": 4080, + "leg_joint4": 1400, + "leg_joint5": 1400, + }, + damping={ + "leg_joint1": 727, + "leg_joint2": 567, + "leg_joint3": 222, + "leg_joint4": 88, + "leg_joint5": 85, + }, ), + # --- Head pan / tilt --- "head": ImplicitActuatorCfg( joint_names_expr=["head_joint.*"], - effort_limit_sim=None, - velocity_limit_sim=None, - stiffness=None, - damping=None, + effort_limit_sim=4, + stiffness=80, + damping=4, ), - "arms": ImplicitActuatorCfg( - joint_names_expr=["left_arm_joint.*", "right_arm_joint.*"], - effort_limit_sim=None, - velocity_limit_sim=None, - stiffness=None, - damping=None, + # --- Arm shoulder (joints 1–2, ±180 N·m) --- + # Left/right values are symmetrised (averaged) and rounded to integers. + "arm_shoulder": ImplicitActuatorCfg( + joint_names_expr=["left_arm_joint[12]", "right_arm_joint[12]"], + effort_limit_sim=180, + velocity_limit_sim=3.141593, + stiffness={ + "left_arm_joint1": 3500, + "left_arm_joint2": 3400, + "right_arm_joint1": 3500, + "right_arm_joint2": 3400, + }, + damping={ + "left_arm_joint1": 100, + "left_arm_joint2": 96, + "right_arm_joint1": 100, + "right_arm_joint2": 96, + }, + ), + # --- Arm elbow (joints 3–4, ±90 N·m) --- + "arm_elbow": ImplicitActuatorCfg( + joint_names_expr=["left_arm_joint[34]", "right_arm_joint[34]"], + effort_limit_sim=90, + velocity_limit_sim=3.926991, + stiffness={ + "left_arm_joint3": 1145, + "left_arm_joint4": 1143, + "right_arm_joint3": 1145, + "right_arm_joint4": 1143, + }, + damping=32, + ), + # --- Arm wrist (joints 5–7, ±30 N·m) --- + "arm_wrist": ImplicitActuatorCfg( + joint_names_expr=["left_arm_joint[5-7]", "right_arm_joint[5-7]"], + effort_limit_sim=30, + velocity_limit_sim=3.926991, + stiffness={ + "left_arm_joint5": 286, + "left_arm_joint6": 284, + "left_arm_joint7": 284, + "right_arm_joint5": 286, + "right_arm_joint6": 284, + "right_arm_joint7": 284, + }, + damping=8, ), + # --- DESC grippers (±1.5 N·m hardware rating) --- "grippers": ImplicitActuatorCfg( joint_names_expr=["left_gripper_joint", "right_gripper_joint"], - effort_limit_sim=1.0, - velocity_limit_sim=3.5, - stiffness=40.0, - damping=5.0, + effort_limit_sim=1.5, + stiffness=77, + damping=4, ), + # --- Holonomic wheels: preserve USD-authored drive properties --- "wheels": ImplicitActuatorCfg( joint_names_expr=WHEEL_JOINTS, effort_limit_sim=None, diff --git a/tests/test_galbot_configs.py b/tests/test_galbot_configs.py index 5403b586..8a2ebff7 100644 --- a/tests/test_galbot_configs.py +++ b/tests/test_galbot_configs.py @@ -167,3 +167,48 @@ def __init__(self): assert tuple(asset.init_state.pos) == pytest.approx((0.5, 0.0, 0.0)) assert tuple(asset.init_state.rot) == pytest.approx((0.0, 0.0, 0.0, 1.0)) assert table_fixture_asset(None, _FakeRobotCfg) is None + + +def test_gravity_compensation_is_enabled(): + """disable_gravity must be True to simulate controller-side gravity compensation.""" + robot = GalbotGolfFixedBaseCfg().robot + assert robot.spawn.rigid_props.disable_gravity is True + + +def test_actuator_gains_match_sim2real_profile(): + """Actuator stiffness, damping and effort limits must match galbot-one-golf-sim2real-v1. + + Left/right arm values are symmetrised; grippers use the hardware effort rating. + """ + robot = GalbotGolfFixedBaseCfg().robot + actuators = robot.actuators + + # Shoulder (joints 1–2): highest-torque arm section, ±180 N·m. + shoulder = actuators["arm_shoulder"] + assert shoulder.effort_limit_sim == pytest.approx(180) + assert shoulder.velocity_limit_sim == pytest.approx(3.141593) + assert shoulder.stiffness["left_arm_joint1"] == pytest.approx(3500) + assert shoulder.stiffness["right_arm_joint1"] == pytest.approx(3500) + assert shoulder.stiffness["left_arm_joint2"] == pytest.approx(3400) + assert shoulder.damping["left_arm_joint1"] == pytest.approx(100) + assert shoulder.damping["right_arm_joint1"] == pytest.approx(100) + + # Elbow (joints 3–4): ±90 N·m, uniform damping. + elbow = actuators["arm_elbow"] + assert elbow.effort_limit_sim == pytest.approx(90) + assert elbow.stiffness["left_arm_joint3"] == pytest.approx(1145) + assert elbow.stiffness["right_arm_joint3"] == pytest.approx(1145) + assert elbow.damping == pytest.approx(32) + + # Wrist (joints 5–7): ±30 N·m, uniform damping. + wrist = actuators["arm_wrist"] + assert wrist.effort_limit_sim == pytest.approx(30) + assert wrist.stiffness["left_arm_joint5"] == pytest.approx(286) + assert wrist.stiffness["right_arm_joint5"] == pytest.approx(286) + assert wrist.damping == pytest.approx(8) + + # Grippers: hardware-rated effort limit. + grippers = actuators["grippers"] + assert grippers.effort_limit_sim == pytest.approx(1.5) + assert grippers.stiffness == pytest.approx(77) + assert grippers.damping == pytest.approx(4)