diff --git a/examples/random_pose_visualizer.py b/examples/random_pose_visualizer.py index 535e47b3a..8eda6458a 100644 --- a/examples/random_pose_visualizer.py +++ b/examples/random_pose_visualizer.py @@ -41,9 +41,9 @@ parser.add_argument( "--robot", type=str, - choices=["g1", "rigv1", "smpl"], + choices=["g1", "rigv1", "smpl", "kangaroo"], default="g1", - help="Robot to load (g1, rigv1, or smpl)", + help="Robot to load (g1, rigv1, smpl, or kangaroo)", ) parser.add_argument("--num_envs", type=int, default=1, help="Number of environments") parser.add_argument("--headless", action="store_true", help="Run in headless mode") @@ -95,6 +95,16 @@ class RobotSpec: "right_ankle_roll_link", ], ), + "kangaroo": RobotSpec( + viz_bodies=[ + "base_link", + "pelvis_2_link", + "leg_left_knee_link", + "leg_right_knee_link", + "leg_left_5_link", + "leg_right_5_link", + ], + ), "rigv1": RobotSpec( viz_bodies=["Hips", "Spine2", "LeftLeg", "RightLeg", "LeftFoot", "RightFoot"], ), @@ -202,10 +212,10 @@ def _gen_random_pose(self): print("Generating new random pose") - dof_limits_lower = self.robot_config.kinematic_info.dof_limits_lower.to( + dof_limits_lower = self.robot_cfg.kinematic_info.dof_limits_lower.to( self.device ) - dof_limits_upper = self.robot_config.kinematic_info.dof_limits_upper.to( + dof_limits_upper = self.robot_cfg.kinematic_info.dof_limits_upper.to( self.device ) print("dof_limits_lower=", dof_limits_lower) diff --git a/protomotions/data/assets/mesh/KANGAROO/arm/arm1_link.STL b/protomotions/data/assets/mesh/KANGAROO/arm/arm1_link.STL new file mode 100644 index 000000000..27bf3decb Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/arm/arm1_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/arm/arm2_link.STL b/protomotions/data/assets/mesh/KANGAROO/arm/arm2_link.STL new file mode 100644 index 000000000..ac70f2178 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/arm/arm2_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/arm/arm3_link.STL b/protomotions/data/assets/mesh/KANGAROO/arm/arm3_link.STL new file mode 100644 index 000000000..c83d656a4 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/arm/arm3_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/arm/arm_base_link.STL b/protomotions/data/assets/mesh/KANGAROO/arm/arm_base_link.STL new file mode 100644 index 000000000..ce7e76db7 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/arm/arm_base_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/arm/fake_forearm_left_link.STL b/protomotions/data/assets/mesh/KANGAROO/arm/fake_forearm_left_link.STL new file mode 100644 index 000000000..6b82e77aa Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/arm/fake_forearm_left_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/arm/fake_forearm_right_link.STL b/protomotions/data/assets/mesh/KANGAROO/arm/fake_forearm_right_link.STL new file mode 100644 index 000000000..fde76c41c Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/arm/fake_forearm_right_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/base/base_link_with_pelvis.STL b/protomotions/data/assets/mesh/KANGAROO/base/base_link_with_pelvis.STL new file mode 100644 index 000000000..589455151 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/base/base_link_with_pelvis.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/base/handle.STL b/protomotions/data/assets/mesh/KANGAROO/base/handle.STL new file mode 100644 index 000000000..fee2fad46 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/base/handle.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_femur_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_femur_link.STL new file mode 100644 index 000000000..195b2458b Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_femur_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_1_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_1_link.STL new file mode 100644 index 000000000..75c6090d5 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_1_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_2_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_2_link.STL new file mode 100644 index 000000000..833fd9184 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_2_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_3_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_3_link.STL new file mode 100644 index 000000000..b6c12c77d Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_3_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_5_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_5_link.STL new file mode 100644 index 000000000..52694e4db Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_5_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_6_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_6_link.STL new file mode 100644 index 000000000..f7e82bc5d Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_6_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_7_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_7_link.STL new file mode 100644 index 000000000..28e44a2c8 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_left_7_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_1_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_1_link.STL new file mode 100644 index 000000000..9baf8a8dd Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_1_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_2_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_2_link.STL new file mode 100644 index 000000000..31c6535b1 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_2_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_3_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_3_link.STL new file mode 100644 index 000000000..126db41b5 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_3_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_6_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_6_link.STL new file mode 100644 index 000000000..f7e82bc5d Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_6_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_7_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_7_link.STL new file mode 100644 index 000000000..28e44a2c8 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_right_7_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/leg/leg_tibia_link.STL b/protomotions/data/assets/mesh/KANGAROO/leg/leg_tibia_link.STL new file mode 100644 index 000000000..190aeca89 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/leg/leg_tibia_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_1_link.STL b/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_1_link.STL new file mode 100644 index 000000000..62e3cc1c3 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_1_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_2_link.STL b/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_2_link.STL new file mode 100644 index 000000000..849790174 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_2_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_base_link.STL b/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_base_link.STL new file mode 100644 index 000000000..107b22652 Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/pelvis/pelvis_base_link.STL differ diff --git a/protomotions/data/assets/mesh/KANGAROO/torso/torso_link.STL b/protomotions/data/assets/mesh/KANGAROO/torso/torso_link.STL new file mode 100644 index 000000000..a67ef5aab Binary files /dev/null and b/protomotions/data/assets/mesh/KANGAROO/torso/torso_link.STL differ diff --git a/protomotions/data/assets/mjcf/kangaroo.xml b/protomotions/data/assets/mjcf/kangaroo.xml new file mode 100644 index 000000000..fb4aae633 --- /dev/null +++ b/protomotions/data/assets/mjcf/kangaroo.xml @@ -0,0 +1,298 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/protomotions/robot_configs/factory.py b/protomotions/robot_configs/factory.py index a07c91218..b86b06206 100644 --- a/protomotions/robot_configs/factory.py +++ b/protomotions/robot_configs/factory.py @@ -53,6 +53,10 @@ def robot_config(robot_name: str, **updates) -> RobotConfig: from protomotions.robot_configs.rigv1 import Rigv1RobotConfig config = Rigv1RobotConfig() + elif robot_name == "kangaroo": + from protomotions.robot_configs.kangaroo import KangarooRobotConfig + + config = KangarooRobotConfig() else: raise ValueError(f"Invalid robot name: {robot_name}") diff --git a/protomotions/robot_configs/kangaroo.py b/protomotions/robot_configs/kangaroo.py new file mode 100644 index 000000000..bdff9c59b --- /dev/null +++ b/protomotions/robot_configs/kangaroo.py @@ -0,0 +1,187 @@ +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 + + +# Updated constants based on PAL Robotics KANGAROO specifications +NATURAL_FREQ = 10 * 2.0 * 3.1415926535 # 10Hz +DAMPING_RATIO = 2.0 + +# Armature values calculated from (Factor * Inertia * Gear_Ratio^2) +ARMATURE_S_PLUS = 0.01265 # For Arm 1,2 and Pelvis 1,2 +ARMATURE_S_MINUS = 0.00663 # For Arm 3,4 +ARMATURE_LEGS = 0.01 # Leg armature (assumed as it's a simple model with non linear transmission) + +# Stiffness/Damping for Arms & Pelvis (armature * NATURAL_FREQ^2 and 2.0 * DAMPING_RATIO * armature * NATURAL_FREQ) +STIFFNESS_S_PLUS = 49.94 +DAMPING_S_PLUS = 3.179 + +STIFFNESS_S_MINUS = 26.177 +DAMPING_S_MINUS = 1.666 + +# Leg-specific logic (_calc_leg_params) +def get_leg_damping(stiffness): + return round(2.0 * DAMPING_RATIO * stiffness / NATURAL_FREQ, 3) + + +@dataclass +class KangarooRobotConfig(RobotConfig): + common_naming_to_robot_body_names: Dict[str, List[str]] = field( + default_factory=lambda: { + "all_left_foot_bodies": ["leg_left_foot_link"], + "all_right_foot_bodies": ["leg_right_foot_link"], + "all_left_hand_bodies": ["ee_left_dummy_link"], + "all_right_hand_bodies": ["ee_right_dummy_link"], + "head_body_name": ["head_dummy_link"], + "torso_body_name": ["pelvis_2_link"], + } + ) + + trackable_bodies_subset: List[str] = field( + default_factory=lambda: [ + "pelvis_2_link", + "head_dummy_link", + "leg_right_foot_link", + "leg_left_foot_link", + "ee_left_dummy_link", + "ee_right_dummy_link", + ] + ) + + default_root_height: float = 0.9 + + # TODO: add initial joint positions? + + asset: RobotAssetConfig = field( + default_factory=lambda: RobotAssetConfig( + asset_file_name="mjcf/kangaroo.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={ + # Pelvis / Torso (S_PLUS) + "pelvis_[12]_joint": ControlInfo( + stiffness=STIFFNESS_S_PLUS, + damping=DAMPING_S_PLUS, + effort_limit=50.0, + velocity_limit=3.14, + armature=ARMATURE_S_PLUS, + ), + # Legs 1, 2, 3 (Stiffness 100) + "leg_.*_[123]_joint": ControlInfo( + stiffness=100.0, + damping=get_leg_damping(100.0), + effort_limit=230.0, # Using max of the group for safety or specific per joint + velocity_limit=3.87, + armature=ARMATURE_LEGS, + ), + # Legs 4, 5 (Stiffness 30) + "leg_.*_[45]_joint": ControlInfo( + stiffness=30.0, + damping=get_leg_damping(30.0), + effort_limit=140.0, + velocity_limit=3.87, + armature=ARMATURE_LEGS, + ), + # Leg Length + "leg_.*_length_joint": ControlInfo( + stiffness=1600.0, + damping=get_leg_damping(1600.0), + effort_limit=1100.0, + velocity_limit=10.0, + armature=ARMATURE_LEGS, + ), + # Arm 1, 2 (S_PLUS) + "arm_.*_[12]_joint": ControlInfo( + stiffness=STIFFNESS_S_PLUS, + damping=DAMPING_S_PLUS, + effort_limit=50.0, + velocity_limit=1.95, + armature=ARMATURE_S_PLUS, + ), + # Arm 3, 4 (S_MINUS) + "arm_.*_[34]_joint": ControlInfo( + stiffness=STIFFNESS_S_MINUS, + damping=DAMPING_S_MINUS, + effort_limit=25.0, + velocity_limit=3.25, + armature=ARMATURE_S_MINUS, + ), + # Passive leg joints (closed kinematic chain) + "leg_.*_femur_joint": ControlInfo( + stiffness=0.0, + damping=0.0, + effort_limit=None, + velocity_limit=None, + armature=0.01, + ), + "leg_.*_knee_joint": ControlInfo( + stiffness=0.0, + damping=0.0, + effort_limit=None, + velocity_limit=None, + armature=0.01, + ), + } + ) + ) + + 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, # TODO: verify if in newton similarly to mujoco solref >= 2*dt + decimation=4, + ), + ) + )