diff --git a/.history/README_20260121160713.md b/.history/README_20260121160713.md new file mode 100644 index 0000000..e37489b --- /dev/null +++ b/.history/README_20260121160713.md @@ -0,0 +1,39 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [ ] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Example Usage + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130152721.md b/.history/README_20260130152721.md new file mode 100644 index 0000000..12ffd8e --- /dev/null +++ b/.history/README_20260130152721.md @@ -0,0 +1,39 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Example Usage + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130152909.md b/.history/README_20260130152909.md new file mode 100644 index 0000000..81e4a6c --- /dev/null +++ b/.history/README_20260130152909.md @@ -0,0 +1,48 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Morphology Randomization + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + +```python +#QuadrupedBuilder12Dof + +#QuadrupedBuilder16Dof +``` + + + + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130153042.md b/.history/README_20260130153042.md new file mode 100644 index 0000000..5b36748 --- /dev/null +++ b/.history/README_20260130153042.md @@ -0,0 +1,49 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Morphology Randomization + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + +```python +#QuadrupedBuilder12Dof + + +#QuadrupedBuilder16Dof +``` + + + + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130153232.md b/.history/README_20260130153232.md new file mode 100644 index 0000000..99fdb4a --- /dev/null +++ b/.history/README_20260130153232.md @@ -0,0 +1,56 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Metamorphology + +```python +#QuadrupedBuilder12Dof + + +#QuadrupedBuilder16Dof + +#BipedBuilder + + +``` + + + + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130175534.md b/.history/README_20260130175534.md new file mode 100644 index 0000000..d619d41 --- /dev/null +++ b/.history/README_20260130175534.md @@ -0,0 +1,57 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Metamorphology + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder + + +``` + + + + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130175547.md b/.history/README_20260130175547.md new file mode 100644 index 0000000..8376daf --- /dev/null +++ b/.history/README_20260130175547.md @@ -0,0 +1,57 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Metamorphology + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder +python scripts/biped_scene.py + +``` + + + + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130183848.md b/.history/README_20260130183848.md new file mode 100644 index 0000000..be9e71c --- /dev/null +++ b/.history/README_20260130183848.md @@ -0,0 +1,57 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Metamorphology + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py --num_envs 32 + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder +python scripts/biped_scene.py + +``` + + + + + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260130183856.md b/.history/README_20260130183856.md new file mode 100644 index 0000000..4525919 --- /dev/null +++ b/.history/README_20260130183856.md @@ -0,0 +1,53 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Metamorphology + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py --num_envs 32 + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder +python scripts/biped_scene.py + +``` + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260131225857.md b/.history/README_20260131225857.md new file mode 100644 index 0000000..13e4d2f --- /dev/null +++ b/.history/README_20260131225857.md @@ -0,0 +1,53 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Robots + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py --num_envs 32 + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder +python scripts/biped_scene.py + +``` + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/README_20260131225918.md b/.history/README_20260131225918.md new file mode 100644 index 0000000..13e4d2f --- /dev/null +++ b/.history/README_20260131225918.md @@ -0,0 +1,53 @@ +# metamorphosis + +A minimalist implementation of morphology randomization in Isaac Sim. Can be used with or without IsaacLab. + +## TODO: + +- [x] Preliminary implementation. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [ ] A minimal RL baseline. + +## Instaillation + +Git clone and `pip install -e .`. + +## Setup + +See `scripts/`. + +Note that running independently (without IsaacLab) would require installing Open-USD via `pip install usd-core`. +However, IsaacSim uses a custom USD build and is **incompatible** with the one installed via pip. +If you experience IsaacSim errors after `pip install usd-core`, uninstall it. + + +## Generate Robots + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py --num_envs 32 + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder +python scripts/biped_scene.py + +``` + +**Important** + +Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! + +## Citation +If you use this codebase in your research, please cite: + +``` +@misc{metamorphosis2026, + title={Metamorphosis: A Framework for Procedural Asset Generation in Isaac Sim}, + author={Botian Xu}, + year={2026}, + url={https://github.com/btx0424/metamorphosis} +} +``` + diff --git a/.history/scripts/0_single_20260130152701.py b/.history/scripts/0_single_20260130152701.py new file mode 100644 index 0000000..083b0fd --- /dev/null +++ b/.history/scripts/0_single_20260130152701.py @@ -0,0 +1,74 @@ +import argparse + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from metamorphosis.asset_cfg import ProceduralQuadrupedCfg + + +def design_scene(): + """Designs the scene by spawning ground plane, light, objects and meshes from usd files.""" + # Ground-plane + cfg_ground = sim_utils.GroundPlaneCfg() + cfg_ground.func("/World/defaultGroundPlane", cfg_ground) + + # spawn distant light + cfg_light_distant = sim_utils.DistantLightCfg( + intensity=3000.0, + color=(0.75, 0.75, 0.75), + ) + cfg_light_distant.func("/World/lightDistant", cfg_light_distant, translation=(1, 0, 10)) + + cfg_procedural_quadruped = ProceduralQuadrupedCfg( + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + ) + cfg_procedural_quadruped.func("/World/Robot", cfg_procedural_quadruped, translation=(0, 0, 1.0)) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + # Design scene + design_scene() + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + while simulation_app.is_running(): + # perform step + sim.step() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/.history/scripts/0_single_20260130153040.py b/.history/scripts/0_single_20260130153040.py new file mode 100644 index 0000000..083b0fd --- /dev/null +++ b/.history/scripts/0_single_20260130153040.py @@ -0,0 +1,74 @@ +import argparse + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from metamorphosis.asset_cfg import ProceduralQuadrupedCfg + + +def design_scene(): + """Designs the scene by spawning ground plane, light, objects and meshes from usd files.""" + # Ground-plane + cfg_ground = sim_utils.GroundPlaneCfg() + cfg_ground.func("/World/defaultGroundPlane", cfg_ground) + + # spawn distant light + cfg_light_distant = sim_utils.DistantLightCfg( + intensity=3000.0, + color=(0.75, 0.75, 0.75), + ) + cfg_light_distant.func("/World/lightDistant", cfg_light_distant, translation=(1, 0, 10)) + + cfg_procedural_quadruped = ProceduralQuadrupedCfg( + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + ) + cfg_procedural_quadruped.func("/World/Robot", cfg_procedural_quadruped, translation=(0, 0, 1.0)) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + # Design scene + design_scene() + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + while simulation_app.is_running(): + # perform step + sim.step() + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/.history/scripts/biped_scene_20260130152701.py b/.history/scripts/biped_scene_20260130152701.py new file mode 100644 index 0000000..3f67aa8 --- /dev/null +++ b/.history/scripts/biped_scene_20260130152701.py @@ -0,0 +1,161 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=32, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis dimensions + pelvis_height_range=(0.10, 0.18), + pelvis_width_range=(0.20, 0.30), + pelvis_length_range=(0.10, 0.15), + # Leg lengths + leg_length_range=(0.6, 0.9), + # Leg proportions + shin_ratio_range=(0.9, 1.1), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - standing pose + ".*_hip_yaw_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_pitch_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.5), # Higher initial position to avoid ground collision + ), + actuators={ + # Leg actuators - higher stiffness to maintain standing + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_.*_joint", ".*_knee_joint", ".*_ankle_.*_joint"], + effort_limit_sim=300.0, + stiffness=300.0, # Higher stiffness for stability + damping=20.0, # Higher damping + armature=0.01, + friction=0.01, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params): + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}, {param.pelvis_width:.2f}, {param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130183643.py b/.history/scripts/biped_scene_20260130183643.py new file mode 100644 index 0000000..0a0ccb3 --- /dev/null +++ b/.history/scripts/biped_scene_20260130183643.py @@ -0,0 +1,161 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis dimensions + pelvis_height_range=(0.10, 0.18), + pelvis_width_range=(0.20, 0.30), + pelvis_length_range=(0.10, 0.15), + # Leg lengths + leg_length_range=(0.6, 0.9), + # Leg proportions + shin_ratio_range=(0.9, 1.1), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - standing pose + ".*_hip_yaw_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_pitch_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.5), # Higher initial position to avoid ground collision + ), + actuators={ + # Leg actuators - higher stiffness to maintain standing + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_.*_joint", ".*_knee_joint", ".*_ankle_.*_joint"], + effort_limit_sim=300.0, + stiffness=300.0, # Higher stiffness for stability + damping=20.0, # Higher damping + armature=0.01, + friction=0.01, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params): + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}, {param.pelvis_width:.2f}, {param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130190943.py b/.history/scripts/biped_scene_20260130190943.py new file mode 100644 index 0000000..90dcfb8 --- /dev/null +++ b/.history/scripts/biped_scene_20260130190943.py @@ -0,0 +1,161 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis dimensions + pelvis_height_range=(0.10, 0.18), + pelvis_width_range=(0.20, 0.30), + pelvis_length_range=(0.10, 0.15), + # Leg lengths + leg_length_range=(0.6, 0.9), + # Leg proportions + shin_ratio_range=(0.9, 1.1), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Leg actuators - higher stiffness to maintain standing + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_.*_joint", ".*_knee_joint", ".*_ankle_.*_joint"], + effort_limit_sim=300.0, + stiffness=300.0, # Higher stiffness for stability + damping=20.0, # Higher damping + armature=0.01, + friction=0.01, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params): + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}, {param.pelvis_width:.2f}, {param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130190956.py b/.history/scripts/biped_scene_20260130190956.py new file mode 100644 index 0000000..61ef58b --- /dev/null +++ b/.history/scripts/biped_scene_20260130190956.py @@ -0,0 +1,203 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis dimensions + pelvis_height_range=(0.10, 0.18), + pelvis_width_range=(0.20, 0.30), + pelvis_length_range=(0.10, 0.15), + # Leg lengths + leg_length_range=(0.6, 0.9), + # Leg proportions + shin_ratio_range=(0.9, 1.1), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params): + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}, {param.pelvis_width:.2f}, {param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130191004.py b/.history/scripts/biped_scene_20260130191004.py new file mode 100644 index 0000000..f63a852 --- /dev/null +++ b/.history/scripts/biped_scene_20260130191004.py @@ -0,0 +1,203 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis dimensions - smaller range for more realistic robots + pelvis_height_range=(0.08, 0.12), + pelvis_width_range=(0.18, 0.25), + pelvis_length_range=(0.08, 0.12), + # Leg lengths - closer to human proportions + leg_length_range=(0.5, 0.7), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params): + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}, {param.pelvis_width:.2f}, {param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130191013.py b/.history/scripts/biped_scene_20260130191013.py new file mode 100644 index 0000000..5105925 --- /dev/null +++ b/.history/scripts/biped_scene_20260130191013.py @@ -0,0 +1,206 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis dimensions - smaller range for more realistic robots + pelvis_height_range=(0.08, 0.12), + pelvis_width_range=(0.18, 0.25), + pelvis_length_range=(0.08, 0.12), + # Leg lengths - closer to human proportions + leg_length_range=(0.5, 0.7), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}×{param.pelvis_width:.2f}×{param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}, " + f"masses=(pelvis:{param.pelvis_mass:.1f}, thigh:{param.thigh_mass:.1f}, shin:{param.shin_mass:.1f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130193830.py b/.history/scripts/biped_scene_20260130193830.py new file mode 100644 index 0000000..b711fdb --- /dev/null +++ b/.history/scripts/biped_scene_20260130193830.py @@ -0,0 +1,209 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - sphere + pelvis_radius_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.07), + # Hip spacing and ellipsoids + hip_spacing_range=(0.12, 0.18), + hip_ellipsoid_range=(0.04, 0.08), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.2f}×{param.pelvis_width:.2f}×{param.pelvis_height:.2f}), " + f"leg_length={param.thigh_length + param.shin_length:.2f}, " + f"masses=(pelvis:{param.pelvis_mass:.1f}, thigh:{param.thigh_mass:.1f}, shin:{param.shin_mass:.1f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130193838.py b/.history/scripts/biped_scene_20260130193838.py new file mode 100644 index 0000000..ce7a7d8 --- /dev/null +++ b/.history/scripts/biped_scene_20260130193838.py @@ -0,0 +1,210 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - sphere + pelvis_radius_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.07), + # Hip spacing and ellipsoids + hip_spacing_range=(0.12, 0.18), + hip_ellipsoid_range=(0.04, 0.08), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis_radius={param.pelvis_radius:.3f}, " + f"hip_spacing={param.hip_spacing:.3f}, " + f"leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_dims=({param.hip_length:.2f}×{param.hip_width:.2f}×{param.hip_height:.2f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130194657.py b/.history/scripts/biped_scene_20260130194657.py new file mode 100644 index 0000000..0dd3fd9 --- /dev/null +++ b/.history/scripts/biped_scene_20260130194657.py @@ -0,0 +1,212 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis_radius={param.pelvis_radius:.3f}, " + f"hip_spacing={param.hip_spacing:.3f}, " + f"leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_dims=({param.hip_length:.2f}×{param.hip_width:.2f}×{param.hip_height:.2f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130194706.py b/.history/scripts/biped_scene_20260130194706.py new file mode 100644 index 0000000..184f9fe --- /dev/null +++ b/.history/scripts/biped_scene_20260130194706.py @@ -0,0 +1,211 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130201516.py b/.history/scripts/biped_scene_20260130201516.py new file mode 100644 index 0000000..184f9fe --- /dev/null +++ b/.history/scripts/biped_scene_20260130201516.py @@ -0,0 +1,211 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130202148.py b/.history/scripts/biped_scene_20260130202148.py new file mode 100644 index 0000000..e9ea7b5 --- /dev/null +++ b/.history/scripts/biped_scene_20260130202148.py @@ -0,0 +1,210 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - tall rectangular body + pelvis_length_range=(0.12, 0.18), + pelvis_width_range=(0.16, 0.24), + pelvis_height_range=(0.20, 0.35), # 足够高 + # Hip positioning and capsules + hip_offset_range=(0.06, 0.12), + hip_capsule_length_range=(0.06, 0.10), + hip_capsule_radius_range=(0.02, 0.035), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130202313.py b/.history/scripts/biped_scene_20260130202313.py new file mode 100644 index 0000000..09d793c --- /dev/null +++ b/.history/scripts/biped_scene_20260130202313.py @@ -0,0 +1,210 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - tall rectangular body + pelvis_length_range=(0.12, 0.18), + pelvis_width_range=(0.16, 0.24), + pelvis_height_range=(0.20, 0.35), # 足够高 + # Hip positioning and capsules + hip_offset_range=(0.06, 0.12), + hip_capsule_length_range=(0.06, 0.10), + hip_capsule_radius_range=(0.02, 0.035), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_offset_y={param.hip_offset_y:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130202408.py b/.history/scripts/biped_scene_20260130202408.py new file mode 100644 index 0000000..184f9fe --- /dev/null +++ b/.history/scripts/biped_scene_20260130202408.py @@ -0,0 +1,211 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130202439.py b/.history/scripts/biped_scene_20260130202439.py new file mode 100644 index 0000000..184f9fe --- /dev/null +++ b/.history/scripts/biped_scene_20260130202439.py @@ -0,0 +1,211 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.10), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130203644.py b/.history/scripts/biped_scene_20260130203644.py new file mode 100644 index 0000000..e461e14 --- /dev/null +++ b/.history/scripts/biped_scene_20260130203644.py @@ -0,0 +1,211 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 2.0), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260130204344.py b/.history/scripts/biped_scene_20260130204344.py new file mode 100644 index 0000000..79164d3 --- /dev/null +++ b/.history/scripts/biped_scene_20260130204344.py @@ -0,0 +1,211 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and capsules + hip_spacing_range=(0.12, 0.16), + hip_capsule_range=(0.025, 0.045), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131005841.py b/.history/scripts/biped_scene_20260131005841.py new file mode 100644 index 0000000..92a2c56 --- /dev/null +++ b/.history/scripts/biped_scene_20260131005841.py @@ -0,0 +1,214 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip_capsule=(r={param.hip_capsule_radius:.3f}, l={param.hip_capsule_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131005850.py b/.history/scripts/biped_scene_20260131005850.py new file mode 100644 index 0000000..7517e2a --- /dev/null +++ b/.history/scripts/biped_scene_20260131005850.py @@ -0,0 +1,215 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131020228.py b/.history/scripts/biped_scene_20260131020228.py new file mode 100644 index 0000000..18e9a2c --- /dev/null +++ b/.history/scripts/biped_scene_20260131020228.py @@ -0,0 +1,207 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131020237.py b/.history/scripts/biped_scene_20260131020237.py new file mode 100644 index 0000000..cc47218 --- /dev/null +++ b/.history/scripts/biped_scene_20260131020237.py @@ -0,0 +1,206 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131020605.py b/.history/scripts/biped_scene_20260131020605.py new file mode 100644 index 0000000..96243fa --- /dev/null +++ b/.history/scripts/biped_scene_20260131020605.py @@ -0,0 +1,214 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131020611.py b/.history/scripts/biped_scene_20260131020611.py new file mode 100644 index 0000000..7517e2a --- /dev/null +++ b/.history/scripts/biped_scene_20260131020611.py @@ -0,0 +1,215 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131024435.py b/.history/scripts/biped_scene_20260131024435.py new file mode 100644 index 0000000..7517e2a --- /dev/null +++ b/.history/scripts/biped_scene_20260131024435.py @@ -0,0 +1,215 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131132044.py b/.history/scripts/biped_scene_20260131132044.py new file mode 100644 index 0000000..2016697 --- /dev/null +++ b/.history/scripts/biped_scene_20260131132044.py @@ -0,0 +1,215 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131145621.py b/.history/scripts/biped_scene_20260131145621.py new file mode 100644 index 0000000..2016697 --- /dev/null +++ b/.history/scripts/biped_scene_20260131145621.py @@ -0,0 +1,215 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131151820.py b/.history/scripts/biped_scene_20260131151820.py new file mode 100644 index 0000000..5eec697 --- /dev/null +++ b/.history/scripts/biped_scene_20260131151820.py @@ -0,0 +1,224 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + hip_roll_joint_indices = [ + index + for index, name in enumerate(articulation.joint_names) + if "hip_roll_joint" in name + ] + if hip_roll_joint_indices: + roll_angles = (torch.rand((articulation.num_instances, len(hip_roll_joint_indices)), device=default_joint_pos.device) * (np.pi / 2)) + default_joint_pos[:, hip_roll_joint_indices] = roll_angles + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131160724.py b/.history/scripts/biped_scene_20260131160724.py new file mode 100644 index 0000000..5c176d4 --- /dev/null +++ b/.history/scripts/biped_scene_20260131160724.py @@ -0,0 +1,230 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + hip_pitch_joint_indices = [ + index + for index, name in enumerate(articulation.joint_names) + if "hip_pitch_joint" in name + ] + if hip_pitch_joint_indices: + roll_angles = ( + torch.rand( + (articulation.num_instances, len(hip_pitch_joint_indices)), + device=default_joint_pos.device, + ) + * (np.pi / 2) + ) + default_joint_pos[:, hip_pitch_joint_indices] = roll_angles + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131161257.py b/.history/scripts/biped_scene_20260131161257.py new file mode 100644 index 0000000..997c097 --- /dev/null +++ b/.history/scripts/biped_scene_20260131161257.py @@ -0,0 +1,216 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131173116.py b/.history/scripts/biped_scene_20260131173116.py new file mode 100644 index 0000000..997c097 --- /dev/null +++ b/.history/scripts/biped_scene_20260131173116.py @@ -0,0 +1,216 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + pelvis_length_range=(0.08, 0.14), + pelvis_width_range=(0.14, 0.20), + pelvis_height_range=(0.06, 0.20), + # Waist connection + waist_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip1_length_range=(0.025, 0.045), # Hip1 length range + hip1_radius_range=(0.015, 0.035), # Hip1 radius range + hip2_length_range=(0.025, 0.045), # Hip2 length range + hip2_radius_range=(0.015, 0.035), # Hip2 radius range + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + print(f" Env {i}: pelvis=({param.pelvis_length:.3f}×{param.pelvis_width:.3f}×{param.pelvis_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={param.thigh_length + param.shin_length:.2f}, " + f"hip1=(r={param.hip1_radius:.3f}, l={param.hip1_length:.3f}), " + f"hip2=(r={param.hip2_radius:.3f}, l={param.hip2_length:.3f})") + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131203350.py b/.history/scripts/biped_scene_20260131203350.py new file mode 100644 index 0000000..2bdcc14 --- /dev/null +++ b/.history/scripts/biped_scene_20260131203350.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.06, 0.20), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131204501.py b/.history/scripts/biped_scene_20260131204501.py new file mode 100644 index 0000000..3216b73 --- /dev/null +++ b/.history/scripts/biped_scene_20260131204501.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.10, 0.20), # 提高最小高度从0.06到0.10 + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131204932.py b/.history/scripts/biped_scene_20260131204932.py new file mode 100644 index 0000000..485ab91 --- /dev/null +++ b/.history/scripts/biped_scene_20260131204932.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Pelvis - box dimensions + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.50, 1.00), # 提高最小高度从0.06到0.10 + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131205101.py b/.history/scripts/biped_scene_20260131205101.py new file mode 100644 index 0000000..d585b59 --- /dev/null +++ b/.history/scripts/biped_scene_20260131205101.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131205102.py b/.history/scripts/biped_scene_20260131205102.py new file mode 100644 index 0000000..d585b59 --- /dev/null +++ b/.history/scripts/biped_scene_20260131205102.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131205934.py b/.history/scripts/biped_scene_20260131205934.py new file mode 100644 index 0000000..26d13f4 --- /dev/null +++ b/.history/scripts/biped_scene_20260131205934.py @@ -0,0 +1,222 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters (G1比例: 躯干高度 0.10~0.14m) + torso_length_range=(0.08, 0.12), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.10, 0.14), # 躯干核心高度 + pelvis_height_range=(0.04, 0.06), # 腰部连接圆柱 + # Hip spacing (G1: ±0.064m = 0.128m total) + hip_spacing_range=(0.10, 0.14), + # Hip pitch segment (G1: ~30mm高度段) + hip_pitch_link_length_range=(0.025, 0.035), + hip_pitch_link_radius_range=(0.020, 0.030), + # Hip roll segment (G1: ~30mm段) + hip_roll_link_length_range=(0.025, 0.035), + hip_roll_link_radius_range=(0.020, 0.030), + hip_pitch_link_initroll_range=(0.0, np.pi / 6), # 限制到30度避免过度外扩 + # Leg lengths (G1: thigh=0.17m, shin=0.30m, total=0.47m) + # 比例约束: 总腿长 = 躯干高度 × 3.4~4.2 + leg_length_range=(0.38, 0.56), # 0.10*3.8 ~ 0.14*4.0 + # Leg proportions (G1: shin/thigh = 1.76) + shin_ratio_range=(1.5, 2.0), # 小腿比大腿长1.5~2倍 + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131210314.py b/.history/scripts/biped_scene_20260131210314.py new file mode 100644 index 0000000..d585b59 --- /dev/null +++ b/.history/scripts/biped_scene_20260131210314.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131210315.py b/.history/scripts/biped_scene_20260131210315.py new file mode 100644 index 0000000..d585b59 --- /dev/null +++ b/.history/scripts/biped_scene_20260131210315.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_length_range=(0.08, 0.14), + torso_width_range=(0.14, 0.20), + torso_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_length:.3f}×{param.torso_width:.3f}×{param.torso_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131225205.py b/.history/scripts/biped_scene_20260131225205.py new file mode 100644 index 0000000..11751ef --- /dev/null +++ b/.history/scripts/biped_scene_20260131225205.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_link_length_range=(0.08, 0.14), + torso_link_width_range=(0.14, 0.20), + torso_link_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_hoint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_link_length:.3f}×{param.torso_link_width:.3f}×{param.torso_link_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131225541.py b/.history/scripts/biped_scene_20260131225541.py new file mode 100644 index 0000000..fac6866 --- /dev/null +++ b/.history/scripts/biped_scene_20260131225541.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_link_length_range=(0.08, 0.14), + torso_link_width_range=(0.14, 0.20), + torso_link_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_hoint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_link_length:.3f}×{param.torso_link_width:.3f}×{param.torso_link_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131225552.py b/.history/scripts/biped_scene_20260131225552.py new file mode 100644 index 0000000..86c30dd --- /dev/null +++ b/.history/scripts/biped_scene_20260131225552.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_link_length_range=(0.08, 0.14), + torso_link_width_range=(0.14, 0.20), + torso_link_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_link_length:.3f}×{param.torso_link_width:.3f}×{param.torso_link_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/biped_scene_20260131225759.py b/.history/scripts/biped_scene_20260131225759.py new file mode 100644 index 0000000..86c30dd --- /dev/null +++ b/.history/scripts/biped_scene_20260131225759.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_link_length_range=(0.08, 0.14), + torso_link_width_range=(0.14, 0.20), + torso_link_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_link_length:.3f}×{param.torso_link_width:.3f}×{param.torso_link_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/1_scene.py b/.history/scripts/quadruped_scene_20260130152701.py similarity index 100% rename from scripts/1_scene.py rename to .history/scripts/quadruped_scene_20260130152701.py diff --git a/.history/scripts/quadruped_scene_20260130183815.py b/.history/scripts/quadruped_scene_20260130183815.py new file mode 100644 index 0000000..aee6846 --- /dev/null +++ b/.history/scripts/quadruped_scene_20260130183815.py @@ -0,0 +1,138 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis.") +parser.add_argument("--num_envs", type=int, default=32, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralQuadrupedCfg, QuadrupedBuilder + + +QUADRUPED_CONFIG = ArticulationCfg( + spawn=ProceduralQuadrupedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + ".*_hip_joint": 0.0, + "F[L,R]_thigh_joint": np.pi / 4, + "R[L,R]_thigh_joint": np.pi / 4, + ".*_calf_joint": -np.pi / 2, + }, + pos=(0, 0, 1.0), + ), + actuators={ + ".*": ImplicitActuatorCfg( + joint_names_expr=[".*"], + effort_limit_sim=1000.0, + stiffness=200.0, + damping=2.0, + armature=0.01, + friction=0.01, + ), + }, +) + + +class ProceduralQuadrupedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + """Designs the scene.""" + # jetbot = JETBOT_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Jetbot") + quadruped = QUADRUPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Quadruped") + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Quadruped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + scene_cfg = ProceduralQuadrupedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["quadruped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print(articulation.joint_names) + print(articulation.body_names) + # print(articulation.data.default_mass) + print(articulation.root_physx_view.is_homogeneous) + + builder = QuadrupedBuilder.get_instance() + print(builder.params) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.root_physx_view.set_masses(articulation.data.default_mass.sqrt(), torch.arange(articulation.num_instances)) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + print(contact_sensor.data.net_forces_w) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/.history/scripts/quadwheel_scene_20260130183742.py b/.history/scripts/quadwheel_scene_20260130183742.py new file mode 100644 index 0000000..e8985f9 --- /dev/null +++ b/.history/scripts/quadwheel_scene_20260130183742.py @@ -0,0 +1,164 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis quad-wheel robot.") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralQuadWheelCfg, QuadWheelBuilder + + +QUADWHEEL_CONFIG = ArticulationCfg( + spawn=ProceduralQuadWheelCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + wheel_radius_range=(0.08, 0.15), + wheel_width_range=(0.03, 0.06), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + ".*_hip_joint": 0.0, + "F[L,R]_thigh_joint": np.pi / 4, + "R[L,R]_thigh_joint": np.pi / 4, + ".*_calf_joint": -np.pi / 2, + ".*_wheel_joint": 0.0, + }, + pos=(0, 0, 1.0), + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + effort_limit_sim=1000.0, + stiffness=200.0, + damping=2.0, + armature=0.01, + friction=0.01, + ), + "wheels": ImplicitActuatorCfg( + joint_names_expr=[".*_wheel_joint"], + effort_limit_sim=500.0, + stiffness=0.0, # Wheels use velocity control + damping=10.0, + armature=0.01, + friction=0.1, + ), + }, +) + + +class ProceduralQuadWheelSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + """Designs the scene.""" + quadwheel = QUADWHEEL_CONFIG.replace(prim_path="{ENV_REGEX_NS}/QuadWheel") + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/QuadWheel/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + scene_cfg = ProceduralQuadWheelSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["quadwheel"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print(articulation.joint_names) + print(articulation.body_names) + print(f"Total DOF: {articulation.num_joints}") + print(articulation.root_physx_view.is_homogeneous) + + builder = QuadWheelBuilder.get_instance() + print(builder.params) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.root_physx_view.set_masses(articulation.data.default_mass.sqrt(), torch.arange(articulation.num_instances)) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Create wheel velocity command (forward rolling) + wheel_velocity = torch.zeros((args_cli.num_envs, articulation.num_joints), device=sim.device) + # Set wheel joints to rotate forward (indices 12-15 for the 4 wheels) + wheel_velocity[:, 12:16] = 5.0 # 5 rad/s forward rotation + + # Simulate physics + count = 0 + while simulation_app.is_running(): + # perform step + joint_pos_target = default_joint_pos.clone() + + # Simple demo: make wheels spin after 100 steps + if count > 100: + articulation.set_joint_velocity_target(wheel_velocity) + else: + articulation.set_joint_position_target(joint_pos_target) + + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + if count % 100 == 0: + print(f"Step {count}: Contact forces shape: {contact_sensor.data.net_forces_w.shape}") + count += 1 + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/quadwheel_scene_20260130184223.py b/.history/scripts/quadwheel_scene_20260130184223.py new file mode 100644 index 0000000..e8985f9 --- /dev/null +++ b/.history/scripts/quadwheel_scene_20260130184223.py @@ -0,0 +1,164 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis quad-wheel robot.") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralQuadWheelCfg, QuadWheelBuilder + + +QUADWHEEL_CONFIG = ArticulationCfg( + spawn=ProceduralQuadWheelCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + wheel_radius_range=(0.08, 0.15), + wheel_width_range=(0.03, 0.06), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + ".*_hip_joint": 0.0, + "F[L,R]_thigh_joint": np.pi / 4, + "R[L,R]_thigh_joint": np.pi / 4, + ".*_calf_joint": -np.pi / 2, + ".*_wheel_joint": 0.0, + }, + pos=(0, 0, 1.0), + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + effort_limit_sim=1000.0, + stiffness=200.0, + damping=2.0, + armature=0.01, + friction=0.01, + ), + "wheels": ImplicitActuatorCfg( + joint_names_expr=[".*_wheel_joint"], + effort_limit_sim=500.0, + stiffness=0.0, # Wheels use velocity control + damping=10.0, + armature=0.01, + friction=0.1, + ), + }, +) + + +class ProceduralQuadWheelSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + """Designs the scene.""" + quadwheel = QUADWHEEL_CONFIG.replace(prim_path="{ENV_REGEX_NS}/QuadWheel") + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/QuadWheel/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + scene_cfg = ProceduralQuadWheelSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["quadwheel"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print(articulation.joint_names) + print(articulation.body_names) + print(f"Total DOF: {articulation.num_joints}") + print(articulation.root_physx_view.is_homogeneous) + + builder = QuadWheelBuilder.get_instance() + print(builder.params) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.root_physx_view.set_masses(articulation.data.default_mass.sqrt(), torch.arange(articulation.num_instances)) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Create wheel velocity command (forward rolling) + wheel_velocity = torch.zeros((args_cli.num_envs, articulation.num_joints), device=sim.device) + # Set wheel joints to rotate forward (indices 12-15 for the 4 wheels) + wheel_velocity[:, 12:16] = 5.0 # 5 rad/s forward rotation + + # Simulate physics + count = 0 + while simulation_app.is_running(): + # perform step + joint_pos_target = default_joint_pos.clone() + + # Simple demo: make wheels spin after 100 steps + if count > 100: + articulation.set_joint_velocity_target(wheel_velocity) + else: + articulation.set_joint_position_target(joint_pos_target) + + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + if count % 100 == 0: + print(f"Step {count}: Contact forces shape: {contact_sensor.data.net_forces_w.shape}") + count += 1 + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/scripts/quadwheel_scene_20260130184225.py b/.history/scripts/quadwheel_scene_20260130184225.py new file mode 100644 index 0000000..e8985f9 --- /dev/null +++ b/.history/scripts/quadwheel_scene_20260130184225.py @@ -0,0 +1,164 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis quad-wheel robot.") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralQuadWheelCfg, QuadWheelBuilder + + +QUADWHEEL_CONFIG = ArticulationCfg( + spawn=ProceduralQuadWheelCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + wheel_radius_range=(0.08, 0.15), + wheel_width_range=(0.03, 0.06), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + ".*_hip_joint": 0.0, + "F[L,R]_thigh_joint": np.pi / 4, + "R[L,R]_thigh_joint": np.pi / 4, + ".*_calf_joint": -np.pi / 2, + ".*_wheel_joint": 0.0, + }, + pos=(0, 0, 1.0), + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + effort_limit_sim=1000.0, + stiffness=200.0, + damping=2.0, + armature=0.01, + friction=0.01, + ), + "wheels": ImplicitActuatorCfg( + joint_names_expr=[".*_wheel_joint"], + effort_limit_sim=500.0, + stiffness=0.0, # Wheels use velocity control + damping=10.0, + armature=0.01, + friction=0.1, + ), + }, +) + + +class ProceduralQuadWheelSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + """Designs the scene.""" + quadwheel = QUADWHEEL_CONFIG.replace(prim_path="{ENV_REGEX_NS}/QuadWheel") + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/QuadWheel/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + scene_cfg = ProceduralQuadWheelSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["quadwheel"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print(articulation.joint_names) + print(articulation.body_names) + print(f"Total DOF: {articulation.num_joints}") + print(articulation.root_physx_view.is_homogeneous) + + builder = QuadWheelBuilder.get_instance() + print(builder.params) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.root_physx_view.set_masses(articulation.data.default_mass.sqrt(), torch.arange(articulation.num_instances)) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Create wheel velocity command (forward rolling) + wheel_velocity = torch.zeros((args_cli.num_envs, articulation.num_joints), device=sim.device) + # Set wheel joints to rotate forward (indices 12-15 for the 4 wheels) + wheel_velocity[:, 12:16] = 5.0 # 5 rad/s forward rotation + + # Simulate physics + count = 0 + while simulation_app.is_running(): + # perform step + joint_pos_target = default_joint_pos.clone() + + # Simple demo: make wheels spin after 100 steps + if count > 100: + articulation.set_joint_velocity_target(wheel_velocity) + else: + articulation.set_joint_position_target(joint_pos_target) + + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + if count % 100 == 0: + print(f"Step {count}: Contact forces shape: {contact_sensor.data.net_forces_w.shape}") + count += 1 + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/.history/src/metamorphosis/asset_cfg_20260121160713.py b/.history/src/metamorphosis/asset_cfg_20260121160713.py new file mode 100644 index 0000000..db979ea --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260121160713.py @@ -0,0 +1,73 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260121184428.py b/.history/src/metamorphosis/asset_cfg_20260121184428.py new file mode 100644 index 0000000..aa79549 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260121184428.py @@ -0,0 +1,73 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder +from metamorphosis.builder import BipedBuilder + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260121185313.py b/.history/src/metamorphosis/asset_cfg_20260121185313.py new file mode 100644 index 0000000..aa79549 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260121185313.py @@ -0,0 +1,73 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder +from metamorphosis.builder import BipedBuilder + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260121185535.py b/.history/src/metamorphosis/asset_cfg_20260121185535.py new file mode 100644 index 0000000..6372946 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260121185535.py @@ -0,0 +1,135 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder +from metamorphosis.builder import BipedBuilder + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_height_range=cfg.torso_height_range, + torso_width_range=cfg.torso_width_range, + leg_length_total_range=cfg.leg_length_total_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped.""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_height_range: tuple[float, float] = (0.4, 0.6) + """Range for the torso height.""" + + torso_width_range: tuple[float, float] = (0.2, 0.3) + """Range for the torso width.""" + + leg_length_total_range: tuple[float, float] = (0.6, 1.0) + """Range for the total leg length.""" + + calf_length_ratio: tuple[float, float] = (0.8, 1.0) + """Range for the calf length ratio (relative to thigh length).""" diff --git a/.history/src/metamorphosis/asset_cfg_20260121185720.py b/.history/src/metamorphosis/asset_cfg_20260121185720.py new file mode 100644 index 0000000..6372946 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260121185720.py @@ -0,0 +1,135 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder +from metamorphosis.builder import BipedBuilder + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_height_range=cfg.torso_height_range, + torso_width_range=cfg.torso_width_range, + leg_length_total_range=cfg.leg_length_total_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped.""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_height_range: tuple[float, float] = (0.4, 0.6) + """Range for the torso height.""" + + torso_width_range: tuple[float, float] = (0.2, 0.3) + """Range for the torso width.""" + + leg_length_total_range: tuple[float, float] = (0.6, 1.0) + """Range for the total leg length.""" + + calf_length_ratio: tuple[float, float] = (0.8, 1.0) + """Range for the calf length ratio (relative to thigh length).""" diff --git a/.history/src/metamorphosis/asset_cfg_20260121185731.py b/.history/src/metamorphosis/asset_cfg_20260121185731.py new file mode 100644 index 0000000..6372946 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260121185731.py @@ -0,0 +1,135 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder +from metamorphosis.builder import BipedBuilder + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_height_range=cfg.torso_height_range, + torso_width_range=cfg.torso_width_range, + leg_length_total_range=cfg.leg_length_total_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped.""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_height_range: tuple[float, float] = (0.4, 0.6) + """Range for the torso height.""" + + torso_width_range: tuple[float, float] = (0.2, 0.3) + """Range for the torso width.""" + + leg_length_total_range: tuple[float, float] = (0.6, 1.0) + """Range for the total leg length.""" + + calf_length_ratio: tuple[float, float] = (0.8, 1.0) + """Range for the calf length ratio (relative to thigh length).""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130194007.py b/.history/src/metamorphosis/asset_cfg_20260130194007.py new file mode 100644 index 0000000..6f1d68c --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130194007.py @@ -0,0 +1,216 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_height_range=cfg.pelvis_height_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_length_range=cfg.pelvis_length_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_radius_range: tuple[float, float] = (0.08, 0.12) + """Range for the pelvis radius (sphere) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10) + """Range for hip ellipsoid dimensions (length/width/height) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130194013.py b/.history/src/metamorphosis/asset_cfg_20260130194013.py new file mode 100644 index 0000000..19f1df4 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130194013.py @@ -0,0 +1,217 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_radius_range=cfg.pelvis_radius_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_ellipsoid_range=cfg.hip_ellipsoid_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_radius_range: tuple[float, float] = (0.08, 0.12) + """Range for the pelvis radius (sphere) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10) + """Range for hip ellipsoid dimensions (length/width/height) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130194641.py b/.history/src/metamorphosis/asset_cfg_20260130194641.py new file mode 100644 index 0000000..24086ca --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130194641.py @@ -0,0 +1,223 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_radius_range=cfg.pelvis_radius_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_ellipsoid_range=cfg.hip_ellipsoid_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130194648.py b/.history/src/metamorphosis/asset_cfg_20260130194648.py new file mode 100644 index 0000000..6813b0a --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130194648.py @@ -0,0 +1,225 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_capsule_range=cfg.hip_capsule_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130201516.py b/.history/src/metamorphosis/asset_cfg_20260130201516.py new file mode 100644 index 0000000..6813b0a --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130201516.py @@ -0,0 +1,225 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_capsule_range=cfg.hip_capsule_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130202025.py b/.history/src/metamorphosis/asset_cfg_20260130202025.py new file mode 100644 index 0000000..74fb388 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130202025.py @@ -0,0 +1,231 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_capsule_range=cfg.hip_capsule_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.12, 0.20) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.28) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.25, 0.40) + """Range for the pelvis height (box Z dimension, tall) in meters.""" + + hip_offset_range: tuple[float, float] = (0.08, 0.15) + """Range for the hip joint spacing in meters.""" + + hip_capsule_length_range: tuple[float, float] = (0.08, 0.12) + """Range for each hip capsule segment length in meters.""" + + hip_capsule_radius_range: tuple[float, float] = (0.025, 0.040) + """Range for hip capsule radius in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130202103.py b/.history/src/metamorphosis/asset_cfg_20260130202103.py new file mode 100644 index 0000000..e5faecd --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130202103.py @@ -0,0 +1,231 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_offset_range=cfg.hip_offset_range, + hip_capsule_length_range=cfg.hip_capsule_length_range, + hip_capsule_radius_range=cfg.hip_capsule_radius_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.12, 0.20) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.28) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.25, 0.40) + """Range for the pelvis height (box Z dimension, tall) in meters.""" + + hip_offset_range: tuple[float, float] = (0.08, 0.15) + """Range for the hip joint spacing in meters.""" + + hip_capsule_length_range: tuple[float, float] = (0.08, 0.12) + """Range for each hip capsule segment length in meters.""" + + hip_capsule_radius_range: tuple[float, float] = (0.025, 0.040) + """Range for hip capsule radius in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130202408.py b/.history/src/metamorphosis/asset_cfg_20260130202408.py new file mode 100644 index 0000000..6813b0a --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130202408.py @@ -0,0 +1,225 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_capsule_range=cfg.hip_capsule_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260130202442.py b/.history/src/metamorphosis/asset_cfg_20260130202442.py new file mode 100644 index 0000000..6813b0a --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260130202442.py @@ -0,0 +1,225 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_capsule_range=cfg.hip_capsule_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131011029.py b/.history/src/metamorphosis/asset_cfg_20260131011029.py new file mode 100644 index 0000000..3266771 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131011029.py @@ -0,0 +1,228 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip1_length_range=cfg.hip1_length_range, + hip1_radius_range=cfg.hip1_radius_range, + hip2_length_range=cfg.hip2_length_range, + hip2_radius_range=cfg.hip2_radius_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_capsule_range: tuple[float, float] = (0.03, 0.06) + """Range for hip capsule dimensions (radius and length) in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131011037.py b/.history/src/metamorphosis/asset_cfg_20260131011037.py new file mode 100644 index 0000000..dc0c38c --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131011037.py @@ -0,0 +1,237 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip1_length_range=cfg.hip1_length_range, + hip1_radius_range=cfg.hip1_radius_range, + hip2_length_range=cfg.hip2_length_range, + hip2_radius_range=cfg.hip2_radius_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip1_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip1 segment length in meters.""" + + hip1_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip1 segment radius in meters.""" + + hip2_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip2 segment length in meters.""" + + hip2_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip2 segment radius in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131024435.py b/.history/src/metamorphosis/asset_cfg_20260131024435.py new file mode 100644 index 0000000..dc0c38c --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131024435.py @@ -0,0 +1,237 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip1_length_range=cfg.hip1_length_range, + hip1_radius_range=cfg.hip1_radius_range, + hip2_length_range=cfg.hip2_length_range, + hip2_radius_range=cfg.hip2_radius_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip1_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip1 segment length in meters.""" + + hip1_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip1 segment radius in meters.""" + + hip2_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip2 segment length in meters.""" + + hip2_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip2 segment radius in meters.""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131161252.py b/.history/src/metamorphosis/asset_cfg_20260131161252.py new file mode 100644 index 0000000..e222bfd --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131161252.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip1_length_range=cfg.hip1_length_range, + hip1_radius_range=cfg.hip1_radius_range, + hip2_length_range=cfg.hip2_length_range, + hip2_radius_range=cfg.hip2_radius_range, + hip_pitch_roll_range=cfg.hip_pitch_roll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip1_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip1 segment length in meters.""" + + hip1_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip1 segment radius in meters.""" + + hip2_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip2 segment length in meters.""" + + hip2_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip2 segment radius in meters.""" + + hip_pitch_roll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch roll init angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131173122.py b/.history/src/metamorphosis/asset_cfg_20260131173122.py new file mode 100644 index 0000000..e222bfd --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131173122.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + pelvis_length_range=cfg.pelvis_length_range, + pelvis_width_range=cfg.pelvis_width_range, + pelvis_height_range=cfg.pelvis_height_range, + waist_height_range=cfg.waist_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip1_length_range=cfg.hip1_length_range, + hip1_radius_range=cfg.hip1_radius_range, + hip2_length_range=cfg.hip2_length_range, + hip2_radius_range=cfg.hip2_radius_range, + hip_pitch_roll_range=cfg.hip_pitch_roll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + pelvis_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the pelvis length (box X dimension) in meters.""" + + pelvis_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the pelvis width (box Y dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the pelvis height (box Z dimension) in meters.""" + + waist_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist connection height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip1_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip1 segment length in meters.""" + + hip1_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip1 segment radius in meters.""" + + hip2_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip2 segment length in meters.""" + + hip2_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip2 segment radius in meters.""" + + hip_pitch_roll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch roll init angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131203122.py b/.history/src/metamorphosis/asset_cfg_20260131203122.py new file mode 100644 index 0000000..ee5442b --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131203122.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_length_range=cfg.torso_length_range, + torso_width_range=cfg.torso_width_range, + torso_height_range=cfg.torso_height_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_pitch_link_length_range=cfg.hip_pitch_link_length_range, + hip_pitch_link_radius_range=cfg.hip_pitch_link_radius_range, + hip_roll_link_length_range=cfg.hip_roll_link_length_range, + hip_roll_link_radius_range=cfg.hip_roll_link_radius_range, + hip_pitch_link_initroll_range=cfg.hip_pitch_link_initroll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the torso length (box X dimension) in meters.""" + + torso_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the torso width (box Y dimension) in meters.""" + + torso_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the torso height (box Z dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist/pelvis cylinder height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_pitch_link length in meters.""" + + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_pitch_link radius in meters.""" + + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_roll_link length in meters.""" + + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_roll_link radius in meters.""" + + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch link initial roll angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131204854.py b/.history/src/metamorphosis/asset_cfg_20260131204854.py new file mode 100644 index 0000000..ee5442b --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131204854.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_length_range=cfg.torso_length_range, + torso_width_range=cfg.torso_width_range, + torso_height_range=cfg.torso_height_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_pitch_link_length_range=cfg.hip_pitch_link_length_range, + hip_pitch_link_radius_range=cfg.hip_pitch_link_radius_range, + hip_roll_link_length_range=cfg.hip_roll_link_length_range, + hip_roll_link_radius_range=cfg.hip_roll_link_radius_range, + hip_pitch_link_initroll_range=cfg.hip_pitch_link_initroll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the torso length (box X dimension) in meters.""" + + torso_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the torso width (box Y dimension) in meters.""" + + torso_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the torso height (box Z dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist/pelvis cylinder height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_pitch_link length in meters.""" + + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_pitch_link radius in meters.""" + + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_roll_link length in meters.""" + + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_roll_link radius in meters.""" + + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch link initial roll angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131225150.py b/.history/src/metamorphosis/asset_cfg_20260131225150.py new file mode 100644 index 0000000..cc237a8 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131225150.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_link_length_range=cfg.torso_link_length_range, + torso_link_width_range=cfg.torso_link_width_range, + torso_link_height_range=cfg.torso_link_height_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_pitch_link_length_range=cfg.hip_pitch_link_length_range, + hip_pitch_link_radius_range=cfg.hip_pitch_link_radius_range, + hip_roll_link_length_range=cfg.hip_roll_link_length_range, + hip_roll_link_radius_range=cfg.hip_roll_link_radius_range, + hip_pitch_link_initroll_range=cfg.hip_pitch_link_initroll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_link_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the torso length (box X dimension) in meters.""" + + torso_link_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the torso width (box Y dimension) in meters.""" + + torso_link_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the torso height (box Z dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist/pelvis cylinder height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_pitch_link length in meters.""" + + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_pitch_link radius in meters.""" + + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_roll_link length in meters.""" + + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_roll_link radius in meters.""" + + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch link initial roll angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131225803.py b/.history/src/metamorphosis/asset_cfg_20260131225803.py new file mode 100644 index 0000000..cc237a8 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131225803.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_link_length_range=cfg.torso_link_length_range, + torso_link_width_range=cfg.torso_link_width_range, + torso_link_height_range=cfg.torso_link_height_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_pitch_link_length_range=cfg.hip_pitch_link_length_range, + hip_pitch_link_radius_range=cfg.hip_pitch_link_radius_range, + hip_roll_link_length_range=cfg.hip_roll_link_length_range, + hip_roll_link_radius_range=cfg.hip_roll_link_radius_range, + hip_pitch_link_initroll_range=cfg.hip_pitch_link_initroll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_link_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the torso length (box X dimension) in meters.""" + + torso_link_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the torso width (box Y dimension) in meters.""" + + torso_link_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the torso height (box Z dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist/pelvis cylinder height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_pitch_link length in meters.""" + + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_pitch_link radius in meters.""" + + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_roll_link length in meters.""" + + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_roll_link radius in meters.""" + + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch link initial roll angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/asset_cfg_20260131233258.py b/.history/src/metamorphosis/asset_cfg_20260131233258.py new file mode 100644 index 0000000..cc237a8 --- /dev/null +++ b/.history/src/metamorphosis/asset_cfg_20260131233258.py @@ -0,0 +1,241 @@ +from isaaclab.sim.spawners import SpawnerCfg +from isaaclab.sim import schemas +from isaaclab.utils.configclass import configclass +from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths +from typing import Callable + +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder + + +def spawn( + prim_path: str, + cfg: "ProceduralQuadrupedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadrupedBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadrupedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quadruped.""" + + func: Callable = spawn + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_link_length_range=cfg.torso_link_length_range, + torso_link_width_range=cfg.torso_link_width_range, + torso_link_height_range=cfg.torso_link_height_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_pitch_link_length_range=cfg.hip_pitch_link_length_range, + hip_pitch_link_radius_range=cfg.hip_pitch_link_radius_range, + hip_roll_link_length_range=cfg.hip_roll_link_length_range, + hip_roll_link_radius_range=cfg.hip_roll_link_radius_range, + hip_pitch_link_initroll_range=cfg.hip_pitch_link_initroll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_link_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the torso length (box X dimension) in meters.""" + + torso_link_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the torso width (box Y dimension) in meters.""" + + torso_link_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the torso height (box Z dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist/pelvis cylinder height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_pitch_link length in meters.""" + + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_pitch_link radius in meters.""" + + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_roll_link length in meters.""" + + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_roll_link radius in meters.""" + + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch link initial roll angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" + + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/.history/src/metamorphosis/builder/__init___20260121160713.py b/.history/src/metamorphosis/builder/__init___20260121160713.py new file mode 100644 index 0000000..6a0b05f --- /dev/null +++ b/.history/src/metamorphosis/builder/__init___20260121160713.py @@ -0,0 +1,3 @@ +from .quadruped import QuadrupedBuilder + +__all__ = ["QuadrupedBuilder"] \ No newline at end of file diff --git a/.history/src/metamorphosis/builder/__init___20260121185305.py b/.history/src/metamorphosis/builder/__init___20260121185305.py new file mode 100644 index 0000000..bfcc5d7 --- /dev/null +++ b/.history/src/metamorphosis/builder/__init___20260121185305.py @@ -0,0 +1,4 @@ +from .quadruped import QuadrupedBuilder +from .biped import BipedBuilder + +__all__ = ["QuadrupedBuilder"] \ No newline at end of file diff --git a/.history/src/metamorphosis/builder/__init___20260121185311.py b/.history/src/metamorphosis/builder/__init___20260121185311.py new file mode 100644 index 0000000..0d30394 --- /dev/null +++ b/.history/src/metamorphosis/builder/__init___20260121185311.py @@ -0,0 +1,4 @@ +from .quadruped import QuadrupedBuilder +from .biped import BipedBuilder + +__all__ = ["QuadrupedBuilder", "BipedBuilder"] \ No newline at end of file diff --git a/.history/src/metamorphosis/builder/__init___20260131233337.py b/.history/src/metamorphosis/builder/__init___20260131233337.py new file mode 100644 index 0000000..b289a25 --- /dev/null +++ b/.history/src/metamorphosis/builder/__init___20260131233337.py @@ -0,0 +1,5 @@ +from .quadruped import QuadrupedBuilder +from .biped import BipedBuilder +from .quadwheel import QuadWheelBuilder + +__all__ = ["QuadrupedBuilder", "BipedBuilder", "QuadWheelBuilder"] diff --git a/.history/src/metamorphosis/builder/biped_20260121160713.py b/.history/src/metamorphosis/builder/biped_20260121160713.py new file mode 100644 index 0000000..e69de29 diff --git a/.history/src/metamorphosis/builder/biped_20260121170841.py b/.history/src/metamorphosis/builder/biped_20260121170841.py new file mode 100644 index 0000000..1cc98da --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260121170841.py @@ -0,0 +1,155 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class QuadrupedParam(NamedTuple): + base_length: float + base_width: float + base_height: float + thigh_length: float + calf_length: float + thigh_radius: float + + +class QuadrupedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized quadruped robots. + + This class creates quadruped robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The quadruped consists of: + - A base body (trunk) with configurable length, width, and height + - Four legs, each with: + - A hip joint (abduction/adduction) + - A thigh segment with a knee joint + - A calf segment + - A foot (spherical contact point) + + Attributes: + base_length_range: Tuple of (min, max) for base body length in meters. + base_width_range: Tuple of (min, max) for base body width in meters. + base_height_range: Tuple of (min, max) for base body height in meters. + leg_length_range: Tuple of (min, max) for leg length as a ratio of base_length. + calf_length_ratio: Tuple of (min, max) for calf length as a ratio of thigh_length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = QuadrupedBuilder( + ... base_length_range=(0.5, 1.0), + ... leg_length_range=(0.4, 0.8) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + base_length_range: tuple[float, float] = (0.5, 1.0), + base_width_range: tuple[float, float] = (0.3, 0.4), + base_height_range: tuple[float, float] = (0.15, 0.25), + leg_length_range: tuple[float, float] = (0.4, 0.8), + calf_length_ratio: tuple[float, float] = (0.9, 1.0), + valid_filter: Callable[[QuadrupedParam], bool] = lambda _: True, + ): + super().__init__() + self.base_length_range = base_length_range + self.base_width_range = base_width_range + self.base_height_range = base_height_range + self.leg_length_range = leg_length_range + self.calf_length_ratio = calf_length_ratio + self.valid_filter = valid_filter + + def sample_params(self, seed: int=-1): + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + base_length = random.uniform(*self.base_length_range) + base_width = random.uniform(*self.base_width_range) + base_height = random.uniform(*self.base_height_range) + thigh_length = base_length * random.uniform(*self.leg_length_range) + calf_length = thigh_length * random.uniform(*self.calf_length_ratio) + thigh_radius = random.uniform(0.03, 0.05) + param = QuadrupedParam( + base_length=base_length, + base_width=base_width, + base_height=base_height, + thigh_length=thigh_length, + calf_length=calf_length, + thigh_radius=thigh_radius, + ) + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + return param + + def generate_mjspec(self, param: QuadrupedParam) -> mujoco.MjSpec: + thigh_radius = param.thigh_radius + calf_radius = param.thigh_radius * 0.8 + foot_radius = param.thigh_radius * 0.9 + + spec = mujoco.MjSpec() + base_body = spec.worldbody.add_body() + base_body.name = "base" + base_body.mass = 1.0 + base_body.inertia = [1.0, 1.0, 1.0] + trunk_geom = base_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + trunk_geom.size = [param.base_length/2, param.base_width/2, param.base_height/2] + + for name, (x, y) in [ + ("FL", (1, -1)), + ("FR", (1, 1)), + ("RL", (-1, -1)), + ("RR", (-1, 1)) + ]: + hip_body = base_body.add_body(name=f"{name}_hip") + hip_body.pos = [0.5 * param.base_length * x, 0.5 * param.base_width * y, 0] + hip_body.mass = 1.0 + hip_body.inertia = [1.0, 1.0, 1.0] + joint = hip_body.add_joint(name=f"{name}_hip_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[1, 0, 0]) + joint.range = [-np.pi * 0.6, +np.pi * 0.6] + add_capsule_geom_(hip_body, radius=thigh_radius * 1.8, fromto=[-0.1, 0, 0, 0.1, 0, 0]) + + thigh_body = hip_body.add_body(name=f"{name}_thigh") + thigh_body.pos = [0.1 * x, 0.1 * y, 0] + thigh_body.mass = 1.0 + thigh_body.inertia = [1.0, 1.0, 1.0] + joint = thigh_body.add_joint(name=f"{name}_thigh_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + joint.range = [-np.pi * 0.8, np.pi * 0.8] + add_capsule_geom_(thigh_body, radius=thigh_radius, fromto=[0, 0, 0, 0, 0, -param.thigh_length]) + + calf_body = thigh_body.add_body(name=f"{name}_calf") + calf_body.pos = [0, 0, -param.thigh_length] + calf_body.mass = 1.0 + calf_body.inertia = [1.0, 1.0, 1.0] + joint = calf_body.add_joint(name=f"{name}_calf_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + joint.range = [-np.pi, 0] + add_capsule_geom_(calf_body, radius=calf_radius, fromto=[0, 0, 0, 0, 0, -param.calf_length]) + + feet_body = calf_body.add_body(name=f"{name}_foot") + feet_body.pos = [0, 0, -param.calf_length] + feet_body.mass = 1.0 + feet_body.inertia = [1.0, 1.0, 1.0] + feet_geom = feet_body.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + feet_geom.size = [foot_radius, 0., 0.] + + return spec + + +if __name__ == "__main__": + builder = QuadrupedBuilder() + builder_instance = QuadrupedBuilder.get_instance() + assert builder_instance is builder + # builder = QuadrupedBuilder() + param = builder.sample_params() + print(param) + diff --git a/.history/src/metamorphosis/builder/biped_20260121170854.py b/.history/src/metamorphosis/builder/biped_20260121170854.py new file mode 100644 index 0000000..c614e5a --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260121170854.py @@ -0,0 +1,155 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + base_length: float + base_width: float + base_height: float + thigh_length: float + calf_length: float + thigh_radius: float + + +class QuadrupedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized quadruped robots. + + This class creates quadruped robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The quadruped consists of: + - A base body (trunk) with configurable length, width, and height + - Four legs, each with: + - A hip joint (abduction/adduction) + - A thigh segment with a knee joint + - A calf segment + - A foot (spherical contact point) + + Attributes: + base_length_range: Tuple of (min, max) for base body length in meters. + base_width_range: Tuple of (min, max) for base body width in meters. + base_height_range: Tuple of (min, max) for base body height in meters. + leg_length_range: Tuple of (min, max) for leg length as a ratio of base_length. + calf_length_ratio: Tuple of (min, max) for calf length as a ratio of thigh_length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = QuadrupedBuilder( + ... base_length_range=(0.5, 1.0), + ... leg_length_range=(0.4, 0.8) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + base_length_range: tuple[float, float] = (0.5, 1.0), + base_width_range: tuple[float, float] = (0.3, 0.4), + base_height_range: tuple[float, float] = (0.15, 0.25), + leg_length_range: tuple[float, float] = (0.4, 0.8), + calf_length_ratio: tuple[float, float] = (0.9, 1.0), + valid_filter: Callable[[QuadrupedParam], bool] = lambda _: True, + ): + super().__init__() + self.base_length_range = base_length_range + self.base_width_range = base_width_range + self.base_height_range = base_height_range + self.leg_length_range = leg_length_range + self.calf_length_ratio = calf_length_ratio + self.valid_filter = valid_filter + + def sample_params(self, seed: int=-1): + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + base_length = random.uniform(*self.base_length_range) + base_width = random.uniform(*self.base_width_range) + base_height = random.uniform(*self.base_height_range) + thigh_length = base_length * random.uniform(*self.leg_length_range) + calf_length = thigh_length * random.uniform(*self.calf_length_ratio) + thigh_radius = random.uniform(0.03, 0.05) + param = QuadrupedParam( + base_length=base_length, + base_width=base_width, + base_height=base_height, + thigh_length=thigh_length, + calf_length=calf_length, + thigh_radius=thigh_radius, + ) + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + return param + + def generate_mjspec(self, param: QuadrupedParam) -> mujoco.MjSpec: + thigh_radius = param.thigh_radius + calf_radius = param.thigh_radius * 0.8 + foot_radius = param.thigh_radius * 0.9 + + spec = mujoco.MjSpec() + base_body = spec.worldbody.add_body() + base_body.name = "base" + base_body.mass = 1.0 + base_body.inertia = [1.0, 1.0, 1.0] + trunk_geom = base_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + trunk_geom.size = [param.base_length/2, param.base_width/2, param.base_height/2] + + for name, (x, y) in [ + ("FL", (1, -1)), + ("FR", (1, 1)), + ("RL", (-1, -1)), + ("RR", (-1, 1)) + ]: + hip_body = base_body.add_body(name=f"{name}_hip") + hip_body.pos = [0.5 * param.base_length * x, 0.5 * param.base_width * y, 0] + hip_body.mass = 1.0 + hip_body.inertia = [1.0, 1.0, 1.0] + joint = hip_body.add_joint(name=f"{name}_hip_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[1, 0, 0]) + joint.range = [-np.pi * 0.6, +np.pi * 0.6] + add_capsule_geom_(hip_body, radius=thigh_radius * 1.8, fromto=[-0.1, 0, 0, 0.1, 0, 0]) + + thigh_body = hip_body.add_body(name=f"{name}_thigh") + thigh_body.pos = [0.1 * x, 0.1 * y, 0] + thigh_body.mass = 1.0 + thigh_body.inertia = [1.0, 1.0, 1.0] + joint = thigh_body.add_joint(name=f"{name}_thigh_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + joint.range = [-np.pi * 0.8, np.pi * 0.8] + add_capsule_geom_(thigh_body, radius=thigh_radius, fromto=[0, 0, 0, 0, 0, -param.thigh_length]) + + calf_body = thigh_body.add_body(name=f"{name}_calf") + calf_body.pos = [0, 0, -param.thigh_length] + calf_body.mass = 1.0 + calf_body.inertia = [1.0, 1.0, 1.0] + joint = calf_body.add_joint(name=f"{name}_calf_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + joint.range = [-np.pi, 0] + add_capsule_geom_(calf_body, radius=calf_radius, fromto=[0, 0, 0, 0, 0, -param.calf_length]) + + feet_body = calf_body.add_body(name=f"{name}_foot") + feet_body.pos = [0, 0, -param.calf_length] + feet_body.mass = 1.0 + feet_body.inertia = [1.0, 1.0, 1.0] + feet_geom = feet_body.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + feet_geom.size = [foot_radius, 0., 0.] + + return spec + + +if __name__ == "__main__": + builder = QuadrupedBuilder() + builder_instance = QuadrupedBuilder.get_instance() + assert builder_instance is builder + # builder = QuadrupedBuilder() + param = builder.sample_params() + print(param) + diff --git a/.history/src/metamorphosis/builder/biped_20260121173201.py b/.history/src/metamorphosis/builder/biped_20260121173201.py new file mode 100644 index 0000000..ef45559 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260121173201.py @@ -0,0 +1,127 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + torso_height: float + torso_width: float + torso_depth: float + thigh_length: float + calf_length: float + thigh_radius: float + foot_length: float + + +class BipedBuilder(BuilderBase): + """ + 针对双足机器人的随机生成器。 + 结构包含：1个躯干 (Torso)，2条腿。 + 每条腿包含：髋关节 (Hip, 2-DOF), 大腿 (Thigh), 小腿 (Calf), 足部 (Foot)。 + """ + + def __init__( + self, + torso_height_range: tuple[float, float] = (0.4, 0.6), + torso_width_range: tuple[float, float] = (0.2, 0.3), + leg_length_total_range: tuple[float, float] = (0.6, 1.0), # 腿部总长范围 + calf_length_ratio: tuple[float, float] = (0.8, 1.0), # 小腿相对于大腿的比例 + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_height_range = torso_height_range + self.torso_width_range = torso_width_range + self.leg_length_total_range = leg_length_total_range + self.calf_length_ratio = calf_length_ratio + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + torso_height = random.uniform(*self.torso_height_range) + torso_width = random.uniform(*self.torso_width_range) + torso_depth = torso_width * 0.6 + + total_leg_len = random.uniform(*self.leg_length_total_range) + ratio = random.uniform(*self.calf_length_ratio) + # thigh + thigh * ratio = total_leg_len + thigh_length = total_leg_len / (1 + ratio) + calf_length = thigh_length * ratio + + thigh_radius = random.uniform(0.03, 0.045) + foot_length = thigh_length * 0.3 + + param = BipedParam( + torso_height=torso_height, + torso_width=torso_width, + torso_depth=torso_depth, + thigh_length=thigh_length, + calf_length=calf_length, + thigh_radius=thigh_radius, + foot_length=foot_length + ) + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + thigh_radius = param.thigh_radius + calf_radius = param.thigh_radius * 0.8 + + spec = mujoco.MjSpec() + # 躯干 (Torso) + torso_body = spec.worldbody.add_body(name="torso") + torso_body.mass = 2.0 + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_depth/2, param.torso_width/2, param.torso_height/2] + + # 左右两条腿 + for name, side_sign in [("L", -1), ("R", 1)]: + # 髋部节点 (Hip - Roll/Abduction) + hip_body = torso_body.add_body(name=f"{name}_hip") + hip_body.pos = [0, (param.torso_width/2) * side_sign, -param.torso_height/2] + + # 髋关节1: Roll (前后摆动方向的垂直轴) + hip_roll = hip_body.add_joint(name=f"{name}_hip_roll", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[1, 0, 0]) + hip_roll.range = [-0.5, 0.5] + + # 大腿 (Thigh - Pitch) + thigh_body = hip_body.add_body(name=f"{name}_thigh") + thigh_body.pos = [0, 0, 0] + thigh_pitch = thigh_body.add_joint(name=f"{name}_thigh_pitch", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + thigh_pitch.range = [-1.5, 1.5] + add_capsule_geom_(thigh_body, radius=thigh_radius, fromto=[0, 0, 0, 0, 0, -param.thigh_length]) + + # 小腿 (Calf) + calf_body = thigh_body.add_body(name=f"{name}_calf") + calf_body.pos = [0, 0, -param.thigh_length] + knee_joint = calf_body.add_joint(name=f"{name}_knee", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + knee_joint.range = [0, 2.0] # 典型的膝盖弯曲范围 + add_capsule_geom_(calf_body, radius=calf_radius, fromto=[0, 0, 0, 0, 0, -param.calf_length]) + + # 足部 (Foot) + foot_body = calf_body.add_body(name=f"{name}_foot") + foot_body.pos = [0, 0, -param.calf_length] + # 这里简化为一个长方体足部，增加稳定性 + foot_geom = foot_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, thigh_radius * 1.2, 0.02] + foot_geom.pos = [param.foot_length/4, 0, -0.01] # 脚掌向前延伸 + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params() + print("Generated Biped Params:", param) + spec = builder.generate_mjspec(param) + # model = spec.compile() # 如果需要编译测试 \ No newline at end of file diff --git a/.history/src/metamorphosis/builder/biped_20260121185105.py b/.history/src/metamorphosis/builder/biped_20260121185105.py new file mode 100644 index 0000000..ef45559 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260121185105.py @@ -0,0 +1,127 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + torso_height: float + torso_width: float + torso_depth: float + thigh_length: float + calf_length: float + thigh_radius: float + foot_length: float + + +class BipedBuilder(BuilderBase): + """ + 针对双足机器人的随机生成器。 + 结构包含：1个躯干 (Torso)，2条腿。 + 每条腿包含：髋关节 (Hip, 2-DOF), 大腿 (Thigh), 小腿 (Calf), 足部 (Foot)。 + """ + + def __init__( + self, + torso_height_range: tuple[float, float] = (0.4, 0.6), + torso_width_range: tuple[float, float] = (0.2, 0.3), + leg_length_total_range: tuple[float, float] = (0.6, 1.0), # 腿部总长范围 + calf_length_ratio: tuple[float, float] = (0.8, 1.0), # 小腿相对于大腿的比例 + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_height_range = torso_height_range + self.torso_width_range = torso_width_range + self.leg_length_total_range = leg_length_total_range + self.calf_length_ratio = calf_length_ratio + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + torso_height = random.uniform(*self.torso_height_range) + torso_width = random.uniform(*self.torso_width_range) + torso_depth = torso_width * 0.6 + + total_leg_len = random.uniform(*self.leg_length_total_range) + ratio = random.uniform(*self.calf_length_ratio) + # thigh + thigh * ratio = total_leg_len + thigh_length = total_leg_len / (1 + ratio) + calf_length = thigh_length * ratio + + thigh_radius = random.uniform(0.03, 0.045) + foot_length = thigh_length * 0.3 + + param = BipedParam( + torso_height=torso_height, + torso_width=torso_width, + torso_depth=torso_depth, + thigh_length=thigh_length, + calf_length=calf_length, + thigh_radius=thigh_radius, + foot_length=foot_length + ) + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + thigh_radius = param.thigh_radius + calf_radius = param.thigh_radius * 0.8 + + spec = mujoco.MjSpec() + # 躯干 (Torso) + torso_body = spec.worldbody.add_body(name="torso") + torso_body.mass = 2.0 + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_depth/2, param.torso_width/2, param.torso_height/2] + + # 左右两条腿 + for name, side_sign in [("L", -1), ("R", 1)]: + # 髋部节点 (Hip - Roll/Abduction) + hip_body = torso_body.add_body(name=f"{name}_hip") + hip_body.pos = [0, (param.torso_width/2) * side_sign, -param.torso_height/2] + + # 髋关节1: Roll (前后摆动方向的垂直轴) + hip_roll = hip_body.add_joint(name=f"{name}_hip_roll", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[1, 0, 0]) + hip_roll.range = [-0.5, 0.5] + + # 大腿 (Thigh - Pitch) + thigh_body = hip_body.add_body(name=f"{name}_thigh") + thigh_body.pos = [0, 0, 0] + thigh_pitch = thigh_body.add_joint(name=f"{name}_thigh_pitch", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + thigh_pitch.range = [-1.5, 1.5] + add_capsule_geom_(thigh_body, radius=thigh_radius, fromto=[0, 0, 0, 0, 0, -param.thigh_length]) + + # 小腿 (Calf) + calf_body = thigh_body.add_body(name=f"{name}_calf") + calf_body.pos = [0, 0, -param.thigh_length] + knee_joint = calf_body.add_joint(name=f"{name}_knee", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + knee_joint.range = [0, 2.0] # 典型的膝盖弯曲范围 + add_capsule_geom_(calf_body, radius=calf_radius, fromto=[0, 0, 0, 0, 0, -param.calf_length]) + + # 足部 (Foot) + foot_body = calf_body.add_body(name=f"{name}_foot") + foot_body.pos = [0, 0, -param.calf_length] + # 这里简化为一个长方体足部，增加稳定性 + foot_geom = foot_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, thigh_radius * 1.2, 0.02] + foot_geom.pos = [param.foot_length/4, 0, -0.01] # 脚掌向前延伸 + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params() + print("Generated Biped Params:", param) + spec = builder.generate_mjspec(param) + # model = spec.compile() # 如果需要编译测试 \ No newline at end of file diff --git a/.history/src/metamorphosis/builder/biped_20260121185251.py b/.history/src/metamorphosis/builder/biped_20260121185251.py new file mode 100644 index 0000000..ef45559 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260121185251.py @@ -0,0 +1,127 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + torso_height: float + torso_width: float + torso_depth: float + thigh_length: float + calf_length: float + thigh_radius: float + foot_length: float + + +class BipedBuilder(BuilderBase): + """ + 针对双足机器人的随机生成器。 + 结构包含：1个躯干 (Torso)，2条腿。 + 每条腿包含：髋关节 (Hip, 2-DOF), 大腿 (Thigh), 小腿 (Calf), 足部 (Foot)。 + """ + + def __init__( + self, + torso_height_range: tuple[float, float] = (0.4, 0.6), + torso_width_range: tuple[float, float] = (0.2, 0.3), + leg_length_total_range: tuple[float, float] = (0.6, 1.0), # 腿部总长范围 + calf_length_ratio: tuple[float, float] = (0.8, 1.0), # 小腿相对于大腿的比例 + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_height_range = torso_height_range + self.torso_width_range = torso_width_range + self.leg_length_total_range = leg_length_total_range + self.calf_length_ratio = calf_length_ratio + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + torso_height = random.uniform(*self.torso_height_range) + torso_width = random.uniform(*self.torso_width_range) + torso_depth = torso_width * 0.6 + + total_leg_len = random.uniform(*self.leg_length_total_range) + ratio = random.uniform(*self.calf_length_ratio) + # thigh + thigh * ratio = total_leg_len + thigh_length = total_leg_len / (1 + ratio) + calf_length = thigh_length * ratio + + thigh_radius = random.uniform(0.03, 0.045) + foot_length = thigh_length * 0.3 + + param = BipedParam( + torso_height=torso_height, + torso_width=torso_width, + torso_depth=torso_depth, + thigh_length=thigh_length, + calf_length=calf_length, + thigh_radius=thigh_radius, + foot_length=foot_length + ) + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + thigh_radius = param.thigh_radius + calf_radius = param.thigh_radius * 0.8 + + spec = mujoco.MjSpec() + # 躯干 (Torso) + torso_body = spec.worldbody.add_body(name="torso") + torso_body.mass = 2.0 + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_depth/2, param.torso_width/2, param.torso_height/2] + + # 左右两条腿 + for name, side_sign in [("L", -1), ("R", 1)]: + # 髋部节点 (Hip - Roll/Abduction) + hip_body = torso_body.add_body(name=f"{name}_hip") + hip_body.pos = [0, (param.torso_width/2) * side_sign, -param.torso_height/2] + + # 髋关节1: Roll (前后摆动方向的垂直轴) + hip_roll = hip_body.add_joint(name=f"{name}_hip_roll", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[1, 0, 0]) + hip_roll.range = [-0.5, 0.5] + + # 大腿 (Thigh - Pitch) + thigh_body = hip_body.add_body(name=f"{name}_thigh") + thigh_body.pos = [0, 0, 0] + thigh_pitch = thigh_body.add_joint(name=f"{name}_thigh_pitch", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + thigh_pitch.range = [-1.5, 1.5] + add_capsule_geom_(thigh_body, radius=thigh_radius, fromto=[0, 0, 0, 0, 0, -param.thigh_length]) + + # 小腿 (Calf) + calf_body = thigh_body.add_body(name=f"{name}_calf") + calf_body.pos = [0, 0, -param.thigh_length] + knee_joint = calf_body.add_joint(name=f"{name}_knee", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + knee_joint.range = [0, 2.0] # 典型的膝盖弯曲范围 + add_capsule_geom_(calf_body, radius=calf_radius, fromto=[0, 0, 0, 0, 0, -param.calf_length]) + + # 足部 (Foot) + foot_body = calf_body.add_body(name=f"{name}_foot") + foot_body.pos = [0, 0, -param.calf_length] + # 这里简化为一个长方体足部，增加稳定性 + foot_geom = foot_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, thigh_radius * 1.2, 0.02] + foot_geom.pos = [param.foot_length/4, 0, -0.01] # 脚掌向前延伸 + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params() + print("Generated Biped Params:", param) + spec = builder.generate_mjspec(param) + # model = spec.compile() # 如果需要编译测试 \ No newline at end of file diff --git a/.history/src/metamorphosis/builder/biped_20260130190752.py b/.history/src/metamorphosis/builder/biped_20260130190752.py new file mode 100644 index 0000000..715f266 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130190752.py @@ -0,0 +1,266 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + + # Hip width (slightly less than pelvis width) + hip_width = pelvis_width * random.uniform(0.85, 0.95) + + # Legs + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + leg_radius = random.uniform(0.03, 0.05) + + # Feet - larger for better stability + foot_length = random.uniform(0.18, 0.28) + foot_width = random.uniform(0.10, 0.14) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + hip_width=hip_width, + thigh_length=thigh_length, + shin_length=shin_length, + leg_radius=leg_radius, + foot_length=foot_length, + foot_width=foot_width, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = 5.0 + pelvis.inertia = [0.5, 0.5, 0.5] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS ============ + # Each leg uses a chain of bodies to achieve multi-DOF hip and ankle + # Structure: pelvis -> hip_yaw -> hip_roll -> thigh (hip_pitch) -> shin (knee) -> ankle_pitch -> foot (ankle_roll) + + for side, y_sign in [("L", -1), ("R", 1)]: + # Hip position (at bottom of pelvis) + hip_pos_y = y_sign * param.hip_width / 2 + hip_pos_z = -param.pelvis_height / 2 + + # --- Hip Yaw (virtual body, rotation around Z) --- + hip_yaw_body = pelvis.add_body(name=f"{side}_hip_yaw") + hip_yaw_body.pos = [0, hip_pos_y, hip_pos_z] + hip_yaw_body.mass = 0.1 # Minimal mass for virtual body + hip_yaw_body.inertia = [0.001, 0.001, 0.001] + + hip_yaw_joint = hip_yaw_body.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] + hip_yaw_joint.range = [-np.pi * 0.3, np.pi * 0.3] + + # --- Hip Roll (virtual body, rotation around X) --- + hip_roll_body = hip_yaw_body.add_body(name=f"{side}_hip_roll") + hip_roll_body.pos = [0, 0, 0] # Same position as yaw + hip_roll_body.mass = 0.1 + hip_roll_body.inertia = [0.001, 0.001, 0.001] + + hip_roll_joint = hip_roll_body.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] + hip_roll_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Thigh (with Hip Pitch joint, rotation around Y) --- + thigh = hip_roll_body.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] + thigh.mass = 2.5 + thigh.inertia = [0.2, 0.2, 0.05] + + hip_pitch_joint = thigh.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] + hip_pitch_joint.range = [-np.pi * 0.6, np.pi * 0.6] + + add_capsule_geom_( + thigh, + radius=param.leg_radius, + fromto=[0, 0, 0, 0, 0, -param.thigh_length] + ) + + # --- Shin (with Knee joint) --- + shin = thigh.add_body(name=f"{side}_shin") + shin.pos = [0, 0, -param.thigh_length] + shin.mass = 1.5 + shin.inertia = [0.1, 0.1, 0.02] + + knee_joint = shin.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] + knee_joint.range = [0, np.pi * 0.75] # Knee only bends backward + + shin_radius = param.leg_radius * 0.85 + add_capsule_geom_( + shin, + radius=shin_radius, + fromto=[0, 0, 0, 0, 0, -param.shin_length] + ) + + # --- Ankle Pitch (virtual body) --- + # Position below shin, accounting for capsule end cap radius + ankle_pitch_body = shin.add_body(name=f"{side}_ankle_pitch") + ankle_pitch_body.pos = [0, 0, -param.shin_length - shin_radius] + ankle_pitch_body.mass = 0.1 + ankle_pitch_body.inertia = [0.001, 0.001, 0.001] + + ankle_pitch_joint = ankle_pitch_body.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] + ankle_pitch_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Foot (with Ankle Roll joint) --- + foot = ankle_pitch_body.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] + foot.mass = 0.4 + foot.inertia = [0.01, 0.01, 0.01] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] + ankle_roll_joint.range = [-np.pi * 0.25, np.pi * 0.25] + + # Foot geometry (box, extends forward) + foot_height = 0.02 + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, foot_height] + foot_geom.pos = [param.foot_length / 4, 0, -foot_height] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130190800.py b/.history/src/metamorphosis/builder/biped_20260130190800.py new file mode 100644 index 0000000..1b08095 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130190800.py @@ -0,0 +1,267 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + + # Hip width (slightly less than pelvis width) + hip_width = pelvis_width * random.uniform(0.85, 0.95) + + # Legs + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + leg_radius = random.uniform(0.03, 0.05) + + # Feet - larger for better stability + foot_length = random.uniform(0.18, 0.28) + foot_width = random.uniform(0.10, 0.14) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + hip_width=hip_width, + thigh_length=thigh_length, + shin_length=shin_length, + leg_radius=leg_radius, + foot_length=foot_length, + foot_width=foot_width, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = 5.0 + pelvis.inertia = [0.5, 0.5, 0.5] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS ============ + # Each leg uses a chain of bodies to achieve multi-DOF hip and ankle + # Structure: pelvis -> hip_yaw -> hip_roll -> thigh (hip_pitch) -> shin (knee) -> ankle_pitch -> foot (ankle_roll) + + for side, y_sign in [("L", -1), ("R", 1)]: + # Hip position (at bottom of pelvis) + hip_pos_y = y_sign * param.hip_width / 2 + hip_pos_z = -param.pelvis_height / 2 + + # --- Hip Yaw (virtual body, rotation around Z) --- + hip_yaw_body = pelvis.add_body(name=f"{side}_hip_yaw") + hip_yaw_body.pos = [0, hip_pos_y, hip_pos_z] + hip_yaw_body.mass = 0.1 # Minimal mass for virtual body + hip_yaw_body.inertia = [0.001, 0.001, 0.001] + + hip_yaw_joint = hip_yaw_body.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] + hip_yaw_joint.range = [-np.pi * 0.3, np.pi * 0.3] + + # --- Hip Roll (virtual body, rotation around X) --- + hip_roll_body = hip_yaw_body.add_body(name=f"{side}_hip_roll") + hip_roll_body.pos = [0, 0, 0] # Same position as yaw + hip_roll_body.mass = 0.1 + hip_roll_body.inertia = [0.001, 0.001, 0.001] + + hip_roll_joint = hip_roll_body.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] + hip_roll_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Thigh (with Hip Pitch joint, rotation around Y) --- + thigh = hip_roll_body.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] + thigh.mass = 2.5 + thigh.inertia = [0.2, 0.2, 0.05] + + hip_pitch_joint = thigh.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] + hip_pitch_joint.range = [-np.pi * 0.6, np.pi * 0.6] + + add_capsule_geom_( + thigh, + radius=param.leg_radius, + fromto=[0, 0, 0, 0, 0, -param.thigh_length] + ) + + # --- Shin (with Knee joint) --- + shin = thigh.add_body(name=f"{side}_shin") + shin.pos = [0, 0, -param.thigh_length] + shin.mass = 1.5 + shin.inertia = [0.1, 0.1, 0.02] + + knee_joint = shin.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] + knee_joint.range = [0, np.pi * 0.75] # Knee only bends backward + + shin_radius = param.leg_radius * 0.85 + add_capsule_geom_( + shin, + radius=shin_radius, + fromto=[0, 0, 0, 0, 0, -param.shin_length] + ) + + # --- Ankle Pitch (virtual body) --- + # Position below shin, accounting for capsule end cap radius + ankle_pitch_body = shin.add_body(name=f"{side}_ankle_pitch") + ankle_pitch_body.pos = [0, 0, -param.shin_length - shin_radius] + ankle_pitch_body.mass = 0.1 + ankle_pitch_body.inertia = [0.001, 0.001, 0.001] + + ankle_pitch_joint = ankle_pitch_body.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] + ankle_pitch_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Foot (with Ankle Roll joint) --- + foot = ankle_pitch_body.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] + foot.mass = 0.4 + foot.inertia = [0.01, 0.01, 0.01] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] + ankle_roll_joint.range = [-np.pi * 0.25, np.pi * 0.25] + + # Foot geometry (box, extends forward) + foot_height = 0.02 + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, foot_height] + foot_geom.pos = [param.foot_length / 4, 0, -foot_height] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130190805.py b/.history/src/metamorphosis/builder/biped_20260130190805.py new file mode 100644 index 0000000..22e0050 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130190805.py @@ -0,0 +1,268 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + + # Hip width (slightly less than pelvis width) + hip_width = pelvis_width * random.uniform(0.85, 0.95) + + # Legs + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + leg_radius = random.uniform(0.03, 0.05) + + # Feet - larger for better stability + foot_length = random.uniform(0.18, 0.28) + foot_width = random.uniform(0.10, 0.14) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + hip_width=hip_width, + thigh_length=thigh_length, + shin_length=shin_length, + leg_radius=leg_radius, + foot_length=foot_length, + foot_width=foot_width, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = 5.0 + pelvis.inertia = [0.5, 0.5, 0.5] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS ============ + # Each leg uses a chain of bodies to achieve multi-DOF hip and ankle + # Structure: pelvis -> hip_yaw -> hip_roll -> thigh (hip_pitch) -> shin (knee) -> ankle_pitch -> foot (ankle_roll) + + for side, y_sign in [("L", -1), ("R", 1)]: + # Hip position (at bottom of pelvis) + hip_pos_y = y_sign * param.hip_width / 2 + hip_pos_z = -param.pelvis_height / 2 + + # --- Hip Yaw (virtual body, rotation around Z) --- + hip_yaw_body = pelvis.add_body(name=f"{side}_hip_yaw") + hip_yaw_body.pos = [0, hip_pos_y, hip_pos_z] + hip_yaw_body.mass = 0.1 # Minimal mass for virtual body + hip_yaw_body.inertia = [0.001, 0.001, 0.001] + + hip_yaw_joint = hip_yaw_body.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] + hip_yaw_joint.range = [-np.pi * 0.3, np.pi * 0.3] + + # --- Hip Roll (virtual body, rotation around X) --- + hip_roll_body = hip_yaw_body.add_body(name=f"{side}_hip_roll") + hip_roll_body.pos = [0, 0, 0] # Same position as yaw + hip_roll_body.mass = 0.1 + hip_roll_body.inertia = [0.001, 0.001, 0.001] + + hip_roll_joint = hip_roll_body.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] + hip_roll_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Thigh (with Hip Pitch joint, rotation around Y) --- + thigh = hip_roll_body.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] + thigh.mass = 2.5 + thigh.inertia = [0.2, 0.2, 0.05] + + hip_pitch_joint = thigh.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] + hip_pitch_joint.range = [-np.pi * 0.6, np.pi * 0.6] + + add_capsule_geom_( + thigh, + radius=param.leg_radius, + fromto=[0, 0, 0, 0, 0, -param.thigh_length] + ) + + # --- Shin (with Knee joint) --- + shin = thigh.add_body(name=f"{side}_shin") + shin.pos = [0, 0, -param.thigh_length] + shin.mass = 1.5 + shin.inertia = [0.1, 0.1, 0.02] + + knee_joint = shin.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] + knee_joint.range = [0, np.pi * 0.75] # Knee only bends backward + + shin_radius = param.leg_radius * 0.85 + add_capsule_geom_( + shin, + radius=shin_radius, + fromto=[0, 0, 0, 0, 0, -param.shin_length] + ) + + # --- Ankle Pitch (virtual body) --- + # Position below shin, accounting for capsule end cap radius + ankle_pitch_body = shin.add_body(name=f"{side}_ankle_pitch") + ankle_pitch_body.pos = [0, 0, -param.shin_length - shin_radius] + ankle_pitch_body.mass = 0.1 + ankle_pitch_body.inertia = [0.001, 0.001, 0.001] + + ankle_pitch_joint = ankle_pitch_body.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] + ankle_pitch_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Foot (with Ankle Roll joint) --- + foot = ankle_pitch_body.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] + foot.mass = 0.4 + foot.inertia = [0.01, 0.01, 0.01] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] + ankle_roll_joint.range = [-np.pi * 0.25, np.pi * 0.25] + + # Foot geometry (box, extends forward) + foot_height = 0.02 + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, foot_height] + foot_geom.pos = [param.foot_length / 4, 0, -foot_height] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130190823.py b/.history/src/metamorphosis/builder/biped_20260130190823.py new file mode 100644 index 0000000..081a0c5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130190823.py @@ -0,0 +1,287 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = 5.0 + pelvis.inertia = [0.5, 0.5, 0.5] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS ============ + # Each leg uses a chain of bodies to achieve multi-DOF hip and ankle + # Structure: pelvis -> hip_yaw -> hip_roll -> thigh (hip_pitch) -> shin (knee) -> ankle_pitch -> foot (ankle_roll) + + for side, y_sign in [("L", -1), ("R", 1)]: + # Hip position (at bottom of pelvis) + hip_pos_y = y_sign * param.hip_width / 2 + hip_pos_z = -param.pelvis_height / 2 + + # --- Hip Yaw (virtual body, rotation around Z) --- + hip_yaw_body = pelvis.add_body(name=f"{side}_hip_yaw") + hip_yaw_body.pos = [0, hip_pos_y, hip_pos_z] + hip_yaw_body.mass = 0.1 # Minimal mass for virtual body + hip_yaw_body.inertia = [0.001, 0.001, 0.001] + + hip_yaw_joint = hip_yaw_body.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] + hip_yaw_joint.range = [-np.pi * 0.3, np.pi * 0.3] + + # --- Hip Roll (virtual body, rotation around X) --- + hip_roll_body = hip_yaw_body.add_body(name=f"{side}_hip_roll") + hip_roll_body.pos = [0, 0, 0] # Same position as yaw + hip_roll_body.mass = 0.1 + hip_roll_body.inertia = [0.001, 0.001, 0.001] + + hip_roll_joint = hip_roll_body.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] + hip_roll_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Thigh (with Hip Pitch joint, rotation around Y) --- + thigh = hip_roll_body.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] + thigh.mass = 2.5 + thigh.inertia = [0.2, 0.2, 0.05] + + hip_pitch_joint = thigh.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] + hip_pitch_joint.range = [-np.pi * 0.6, np.pi * 0.6] + + add_capsule_geom_( + thigh, + radius=param.leg_radius, + fromto=[0, 0, 0, 0, 0, -param.thigh_length] + ) + + # --- Shin (with Knee joint) --- + shin = thigh.add_body(name=f"{side}_shin") + shin.pos = [0, 0, -param.thigh_length] + shin.mass = 1.5 + shin.inertia = [0.1, 0.1, 0.02] + + knee_joint = shin.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] + knee_joint.range = [0, np.pi * 0.75] # Knee only bends backward + + shin_radius = param.leg_radius * 0.85 + add_capsule_geom_( + shin, + radius=shin_radius, + fromto=[0, 0, 0, 0, 0, -param.shin_length] + ) + + # --- Ankle Pitch (virtual body) --- + # Position below shin, accounting for capsule end cap radius + ankle_pitch_body = shin.add_body(name=f"{side}_ankle_pitch") + ankle_pitch_body.pos = [0, 0, -param.shin_length - shin_radius] + ankle_pitch_body.mass = 0.1 + ankle_pitch_body.inertia = [0.001, 0.001, 0.001] + + ankle_pitch_joint = ankle_pitch_body.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] + ankle_pitch_joint.range = [-np.pi * 0.4, np.pi * 0.4] + + # --- Foot (with Ankle Roll joint) --- + foot = ankle_pitch_body.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] + foot.mass = 0.4 + foot.inertia = [0.01, 0.01, 0.01] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] + ankle_roll_joint.range = [-np.pi * 0.25, np.pi * 0.25] + + # Foot geometry (box, extends forward) + foot_height = 0.02 + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, foot_height] + foot_geom.pos = [param.foot_length / 4, 0, -foot_height] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130190926.py b/.history/src/metamorphosis/builder/biped_20260130190926.py new file mode 100644 index 0000000..e401915 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130190926.py @@ -0,0 +1,316 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06] + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130191442.py b/.history/src/metamorphosis/builder/biped_20260130191442.py new file mode 100644 index 0000000..7766b1f --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130191442.py @@ -0,0 +1,316 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130191449.py b/.history/src/metamorphosis/builder/biped_20260130191449.py new file mode 100644 index 0000000..ca32549 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130191449.py @@ -0,0 +1,316 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130191453.py b/.history/src/metamorphosis/builder/biped_20260130191453.py new file mode 100644 index 0000000..fc1bfe1 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130191453.py @@ -0,0 +1,316 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot following G1-style design.""" + # Pelvis dimensions (main body) + pelvis_length: float # 骨盆长度 (前后) + pelvis_width: float # 骨盆宽度 (左右) + pelvis_height: float # 骨盆高度 (上下) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130193639.py b/.history/src/metamorphosis/builder/biped_20260130193639.py new file mode 100644 index 0000000..334379c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130193639.py @@ -0,0 +1,320 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_height_range: tuple[float, float] = (0.10, 0.18), + pelvis_width_range: tuple[float, float] = (0.20, 0.30), + pelvis_length_range: tuple[float, float] = (0.10, 0.15), + pelvis_mass_range: tuple[float, float] = (3.5, 5.0), + leg_length_range: tuple[float, float] = (0.6, 0.9), + shin_ratio_range: tuple[float, float] = (0.9, 1.1), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130193649.py b/.history/src/metamorphosis/builder/biped_20260130193649.py new file mode 100644 index 0000000..6e839b2 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130193649.py @@ -0,0 +1,320 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_height_range = pelvis_height_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_length_range = pelvis_length_range + self.pelvis_mass_range = pelvis_mass_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130193654.py b/.history/src/metamorphosis/builder/biped_20260130193654.py new file mode 100644 index 0000000..22a5cc6 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130193654.py @@ -0,0 +1,320 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions and mass + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_mass = random.uniform(*self.pelvis_mass_range) + + # Hip positioning (G1-style offsets) + hip_offset_x = random.uniform(0.0, 0.02) # Slight forward offset + hip_offset_y = pelvis_width * random.uniform(0.35, 0.45) # Hip spacing + hip_offset_z = random.uniform(-0.12, -0.08) # Below pelvis center + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.035, 0.055) + thigh_mass = thigh_length * random.uniform(1.8, 2.5) # Mass proportional to length + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.5, 2.2) + + # Foot parameters (larger for stability) + foot_length = random.uniform(0.20, 0.30) + foot_width = random.uniform(0.08, 0.12) + foot_height = random.uniform(0.025, 0.035) + foot_mass = random.uniform(0.3, 0.6) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130193716.py b/.history/src/metamorphosis/builder/biped_20260130193716.py new file mode 100644 index 0000000..14f8a2b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130193716.py @@ -0,0 +1,326 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Inertia proportional to dimensions and mass + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (G1-style joint structure) ============ + # Each leg: pelvis -> hip_pitch -> hip_roll -> hip_yaw -> thigh -> knee -> shin -> ankle_pitch -> ankle_roll -> foot + + for side, y_sign in [("left", 1), ("right", -1)]: + # Hip position + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Pitch Link --- + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pos + hip_pitch_link.mass = 1.35 # G1-style mass + hip_pitch_link.inertia = [0.0018, 0.0015, 0.0012] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis + hip_pitch_joint.range = [-2.5307, 2.8798] # G1 ranges + hip_pitch_joint.armature = 0.01017752004 + + # --- Hip Roll Link --- + hip_roll_offset = [0, 0.052 * y_sign, -0.030465] # G1-style offset + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = hip_roll_offset + hip_roll_link.mass = 1.52 + hip_roll_link.inertia = [0.0025, 0.0024, 0.0015] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis + # Different ranges for left/right to match G1 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip collision geometry + hip_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_geom.size = [0.06, 0, 0] # [radius, 0, 0] for capsule + hip_geom.fromto = [0.02, 0, 0, 0.02, 0, -0.08] + + # --- Hip Yaw Link (Thigh) --- + hip_yaw_offset = [0.025, 0, -0.12412] # G1-style offset + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = hip_yaw_offset + hip_yaw_link.mass = param.thigh_mass + # Thigh inertia based on dimensions + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh collision geometry + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, -0.03, -0.06, 0, -param.thigh_length] + + # --- Knee Link (Shin) --- + knee_offset = [-0.078273, 0.0021489 * y_sign, -param.thigh_length] + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = knee_offset + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin collision geometry + shin_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0.01, 0, 0, 0.01, 0, -param.shin_length] + + # --- Ankle Pitch Link --- + ankle_pitch_offset = [0, -9.4445e-05 * y_sign, -param.shin_length] + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = ankle_pitch_offset + ankle_pitch_link.mass = 0.52 + ankle_pitch_link.inertia = [0.00067, 0.00067, 0.00027] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- Ankle Roll Link (Foot) --- + ankle_roll_offset = [0, 0, -0.045001] + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = ankle_roll_offset + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry (box extending forward) + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length / 2, param.foot_width / 2, param.foot_height / 2] + foot_geom.pos = [param.foot_length / 4, 0, -param.foot_height / 2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130193820.py b/.history/src/metamorphosis/builder/biped_20260130193820.py new file mode 100644 index 0000000..9f9d430 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130193820.py @@ -0,0 +1,346 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_ELLIPSOID) + hip_pitch_geom.size = [param.hip_length/2, param.hip_width/2, param.hip_height/2] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_ELLIPSOID) + hip_roll_geom.size = [param.hip_length/2, param.hip_width/2, param.hip_height/2] + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_ELLIPSOID) + hip_yaw_geom.size = [param.hip_length/2, param.hip_width/2, param.hip_height/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194111.py b/.history/src/metamorphosis/builder/biped_20260130194111.py new file mode 100644 index 0000000..59c404b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194111.py @@ -0,0 +1,347 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_ELLIPSOID) + hip_roll_geom.size = [param.hip_length/2, param.hip_width/2, param.hip_height/2] + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_ELLIPSOID) + hip_yaw_geom.size = [param.hip_length/2, param.hip_width/2, param.hip_height/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194117.py b/.history/src/metamorphosis/builder/biped_20260130194117.py new file mode 100644 index 0000000..753140b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194117.py @@ -0,0 +1,348 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_ELLIPSOID) + hip_yaw_geom.size = [param.hip_length/2, param.hip_width/2, param.hip_height/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194123.py b/.history/src/metamorphosis/builder/biped_20260130194123.py new file mode 100644 index 0000000..3c48711 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194123.py @@ -0,0 +1,349 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with waist and 6-DOF legs.""" + # Pelvis (main body - sphere) + pelvis_radius: float # 球形骨盆半径 + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + hip_length: float # 椭球髋关节长度 + hip_width: float # 椭球髋关节宽度 + hip_height: float # 椭球髋关节高度 + hip_mass: float # 每个髋关节椭球质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194401.py b/.history/src/metamorphosis/builder/biped_20260130194401.py new file mode 100644 index 0000000..5d79fc3 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194401.py @@ -0,0 +1,350 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_radius_range: tuple[float, float] = (0.08, 0.12), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_ellipsoid_range: tuple[float, float] = (0.06, 0.10), # For length/width/height + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194410.py b/.history/src/metamorphosis/builder/biped_20260130194410.py new file mode 100644 index 0000000..6c5b4b0 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194410.py @@ -0,0 +1,352 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_radius_range = pelvis_radius_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_ellipsoid_range = hip_ellipsoid_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194416.py b/.history/src/metamorphosis/builder/biped_20260130194416.py new file mode 100644 index 0000000..7bf1960 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194416.py @@ -0,0 +1,354 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (sphere) + pelvis_radius = random.uniform(*self.pelvis_radius_range) + pelvis_mass = random.uniform(3.0, 5.0) # Based on radius + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = pelvis_radius * random.uniform(0.3, 0.5) # Smaller than pelvis + + # Hip spacing and ellipsoid dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_length = random.uniform(*self.hip_ellipsoid_range) # X direction + hip_width = random.uniform(*self.hip_ellipsoid_range) # Y direction + hip_height = random.uniform(*self.hip_ellipsoid_range) # Z direction + hip_mass = random.uniform(0.8, 1.5) # Each hip ellipsoid + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_radius=pelvis_radius, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_length=hip_length, + hip_width=hip_width, + hip_height=hip_height, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194437.py b/.history/src/metamorphosis/builder/biped_20260130194437.py new file mode 100644 index 0000000..61429ee --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194437.py @@ -0,0 +1,356 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - sphere) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Sphere inertia: I = (2/5) * m * r^2 + pelvis_inertia = (2/5) * param.pelvis_mass * param.pelvis_radius**2 + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130194630.py b/.history/src/metamorphosis/builder/biped_20260130194630.py new file mode 100644 index 0000000..e997c2c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130194630.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130201516.py b/.history/src/metamorphosis/builder/biped_20260130201516.py new file mode 100644 index 0000000..e997c2c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130201516.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130201757.py b/.history/src/metamorphosis/builder/biped_20260130201757.py new file mode 100644 index 0000000..0dd9020 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130201757.py @@ -0,0 +1,537 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with tall box pelvis and 3-segment capsule hips.""" + # Pelvis (tall rectangular body) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向，要足够高) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Hip capsule segments (三段长胶囊首尾相连) + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_capsule_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130201825.py b/.history/src/metamorphosis/builder/biped_20260130201825.py new file mode 100644 index 0000000..0e67d6e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130201825.py @@ -0,0 +1,537 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with tall box pelvis and 3-segment capsule hips.""" + # Pelvis (tall rectangular body) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向，要足够高) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Hip capsule segments (三段长胶囊首尾相连) + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_capsule_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.12, 0.20), + pelvis_width_range: tuple[float, float] = (0.18, 0.28), + pelvis_height_range: tuple[float, float] = (0.25, 0.40), # 更高的pelvis + hip_offset_range: tuple[float, float] = (0.08, 0.15), # Hip偏移范围 + hip_capsule_length_range: tuple[float, float] = (0.08, 0.12), # 长胶囊 + hip_capsule_radius_range: tuple[float, float] = (0.025, 0.040), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.hip_offset_range = hip_offset_range + self.hip_capsule_length_range = hip_capsule_length_range + self.hip_capsule_radius_range = hip_capsule_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130201919.py b/.history/src/metamorphosis/builder/biped_20260130201919.py new file mode 100644 index 0000000..224d7bd --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130201919.py @@ -0,0 +1,578 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with tall box pelvis and 3-segment capsule hips.""" + # Pelvis (tall rectangular body) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向，要足够高) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Hip capsule segments (三段长胶囊首尾相连) + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_capsule_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.12, 0.20), + pelvis_width_range: tuple[float, float] = (0.18, 0.28), + pelvis_height_range: tuple[float, float] = (0.25, 0.40), # 更高的pelvis + hip_offset_range: tuple[float, float] = (0.08, 0.15), # Hip偏移范围 + hip_capsule_length_range: tuple[float, float] = (0.08, 0.12), # 长胶囊 + hip_capsule_radius_range: tuple[float, float] = (0.025, 0.040), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.hip_offset_range = hip_offset_range + self.hip_capsule_length_range = hip_capsule_length_range + self.hip_capsule_radius_range = hip_capsule_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions (tall rectangular body) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) # 足够高 + pelvis_mass = random.uniform(4.0, 7.0) # 基于尺寸 + + # Hip positioning + hip_offset_x = random.uniform(0.0, 0.02) # 前后偏移 + hip_offset_y = random.uniform(*self.hip_offset_range) # 左右间距的一半 + hip_offset_z = -pelvis_height / 2 + random.uniform(0.02, 0.08) # 在pelvis底部附近 + + # Hip capsule segments (三段长胶囊) + hip_capsule_radius = random.uniform(*self.hip_capsule_radius_range) + hip_capsule_length = random.uniform(*self.hip_capsule_length_range) # 长胶囊 + hip_capsule_mass = random.uniform(0.8, 1.5) # 每段胶囊 + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_capsule_mass=hip_capsule_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130202002.py b/.history/src/metamorphosis/builder/biped_20260130202002.py new file mode 100644 index 0000000..f89b264 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130202002.py @@ -0,0 +1,732 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with tall box pelvis and 3-segment capsule hips.""" + # Pelvis (tall rectangular body) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向，要足够高) + pelvis_mass: float # 骨盆质量 + + # Hip positioning + hip_offset_x: float # 髋关节前后偏移 + hip_offset_y: float # 髋关节左右间距的一半 + hip_offset_z: float # 髋关节垂直偏移（相对骨盆底部） + + # Hip capsule segments (三段长胶囊首尾相连) + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_capsule_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.12, 0.20), + pelvis_width_range: tuple[float, float] = (0.18, 0.28), + pelvis_height_range: tuple[float, float] = (0.25, 0.40), # 更高的pelvis + hip_offset_range: tuple[float, float] = (0.08, 0.15), # Hip偏移范围 + hip_capsule_length_range: tuple[float, float] = (0.08, 0.12), # 长胶囊 + hip_capsule_radius_range: tuple[float, float] = (0.025, 0.040), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.hip_offset_range = hip_offset_range + self.hip_capsule_length_range = hip_capsule_length_range + self.hip_capsule_radius_range = hip_capsule_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis dimensions (tall rectangular body) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) # 足够高 + pelvis_mass = random.uniform(4.0, 7.0) # 基于尺寸 + + # Hip positioning + hip_offset_x = random.uniform(0.0, 0.02) # 前后偏移 + hip_offset_y = random.uniform(*self.hip_offset_range) # 左右间距的一半 + hip_offset_z = -pelvis_height / 2 + random.uniform(0.02, 0.08) # 在pelvis底部附近 + + # Hip capsule segments (三段长胶囊) + hip_capsule_radius = random.uniform(*self.hip_capsule_radius_range) + hip_capsule_length = random.uniform(*self.hip_capsule_length_range) # 长胶囊 + hip_capsule_mass = random.uniform(0.8, 1.5) # 每段胶囊 + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + hip_offset_x=hip_offset_x, + hip_offset_y=hip_offset_y, + hip_offset_z=hip_offset_z, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_capsule_mass=hip_capsule_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - tall rectangular box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Tall pelvis geometry + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length / 2, param.pelvis_width / 2, param.pelvis_height / 2] + + # ============ LEGS (6 DOF each: 3段胶囊hip + knee + 2段ankle) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # Hip 起始位置 + hip_pos = [param.hip_offset_x, y_sign * param.hip_offset_y, param.hip_offset_z] + + # --- Hip Segment 1: PITCH (第一段长胶囊, Y轴旋转) --- + hip_pitch = pelvis.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pos + hip_pitch.mass = param.hip_capsule_mass + # 胶囊惯性 + capsule_ixx = param.hip_capsule_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + capsule_iyy = param.hip_capsule_mass * param.hip_capsule_radius**2 / 2 + hip_pitch.inertia = [capsule_ixx, capsule_iyy, capsule_ixx] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # 第一段胶囊几何体 (沿Z轴向下) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [0, 0, 0, 0, 0, -param.hip_capsule_length] + + # --- Hip Segment 2: ROLL (第二段长胶囊, X轴旋转) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length] # 连接到第一段末端 + hip_roll.mass = param.hip_capsule_mass + hip_roll.inertia = [capsule_ixx, capsule_iyy, capsule_ixx] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # 左右腿不同范围 + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # 第二段胶囊几何体 (沿Z轴向下) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, 0, 0, 0, 0, -param.hip_capsule_length] + + # --- Hip Segment 3: YAW (第三段长胶囊, Z轴旋转) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length] # 连接到第二段末端 + hip_yaw.mass = param.hip_capsule_mass + hip_yaw.inertia = [capsule_ixx, capsule_iyy, capsule_ixx] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # 第三段胶囊几何体 (沿Z轴向下) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_capsule_length] + + # --- THIGH (连接到第三段hip末端) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip_capsule_length] # 连接到第三段末端 + thigh.mass = param.thigh_mass + # 大腿惯性 + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_ixx] + + # 大腿几何体 + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (膝关节 - pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # 小腿惯性 + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + knee.inertia = [shin_ixx, shin_iyy, shin_ixx] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # 小腿几何体 + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # 小的中间质量 + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # 同位置 + foot.mass = param.foot_mass + # 脚惯性 + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # 脚几何体 - 向前延伸的盒子 + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130202408.py b/.history/src/metamorphosis/builder/biped_20260130202408.py new file mode 100644 index 0000000..e997c2c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130202408.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130202437.py b/.history/src/metamorphosis/builder/biped_20260130202437.py new file mode 100644 index 0000000..e997c2c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130202437.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 0.14), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130202833.py b/.history/src/metamorphosis/builder/biped_20260130202833.py new file mode 100644 index 0000000..e20e778 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130202833.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130203359.py b/.history/src/metamorphosis/builder/biped_20260130203359.py new file mode 100644 index 0000000..f5239d4 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130203359.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [param.hip_capsule_length/2, 0, 0] # Offset along X-axis (串联) + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, param.hip_capsule_length/2, 0] # Offset along Y-axis (串联) + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip_capsule_length/2] # Below hip yaw (下连接到大腿) + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130203521.py b/.history/src/metamorphosis/builder/biped_20260130203521.py new file mode 100644 index 0000000..e20e778 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130203521.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130203523.py b/.history/src/metamorphosis/builder/biped_20260130203523.py new file mode 100644 index 0000000..e20e778 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130203523.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130204131.py b/.history/src/metamorphosis/builder/biped_20260130204131.py new file mode 100644 index 0000000..c4625ab --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130204131.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [param.hip_capsule_length/2, 0, 0] # At the end of pitch capsule (X direction) + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130204137.py b/.history/src/metamorphosis/builder/biped_20260130204137.py new file mode 100644 index 0000000..fad8377 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130204137.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [param.hip_capsule_length/2, 0, 0] # At the end of pitch capsule (X direction) + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, param.hip_capsule_length/2, 0] # At the end of roll capsule (Y direction) + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130204147.py b/.history/src/metamorphosis/builder/biped_20260130204147.py new file mode 100644 index 0000000..1cc62e5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130204147.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [param.hip_capsule_length/2, 0, 0] # At the end of pitch capsule (X direction) + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, param.hip_capsule_length/2, 0] # At the end of roll capsule (Y direction) + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip_capsule_length/2] # Below hip yaw (at the end of Z direction) + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130204455.py b/.history/src/metamorphosis/builder/biped_20260130204455.py new file mode 100644 index 0000000..e20e778 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130204455.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130204457.py b/.history/src/metamorphosis/builder/biped_20260130204457.py new file mode 100644 index 0000000..e20e778 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130204457.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130205144.py b/.history/src/metamorphosis/builder/biped_20260130205144.py new file mode 100644 index 0000000..bc354cc --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130205144.py @@ -0,0 +1,521 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + # hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + # hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + # hip_yaw.mass = param.hip_mass + # hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + # hip_yaw_joint = hip_yaw.add_joint( + # name=f"{side}_hip_yaw_joint", + # type=mujoco.mjtJoint.mjJNT_HINGE + # ) + # hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + # hip_yaw_joint.range = [-2.7576, 2.7576] + # hip_yaw_joint.armature = 0.01017752004 + + # # Hip yaw capsule geometry (oriented along Z for yaw movement) + # hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + # hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + # hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130205249.py b/.history/src/metamorphosis/builder/biped_20260130205249.py new file mode 100644 index 0000000..05e87f2 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130205249.py @@ -0,0 +1,519 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + # hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + # hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + # hip_yaw.mass = param.hip_mass + # hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + # hip_yaw_joint = hip_yaw.add_joint( + # name=f"{side}_hip_yaw_joint", + # type=mujoco.mjtJoint.mjJNT_HINGE + # ) + # hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + # hip_yaw_joint.range = [-2.7576, 2.7576] + # hip_yaw_joint.armature = 0.01017752004 + + # # Hip yaw capsule geometry (oriented along Z for yaw movement) + # hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + # hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + # hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130205254.py b/.history/src/metamorphosis/builder/biped_20260130205254.py new file mode 100644 index 0000000..eac1c0a --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130205254.py @@ -0,0 +1,519 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + # hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + # hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + # hip_yaw.mass = param.hip_mass + # hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + # hip_yaw_joint = hip_yaw.add_joint( + # name=f"{side}_hip_yaw_joint", + # type=mujoco.mjtJoint.mjJNT_HINGE + # ) + # hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + # hip_yaw_joint.range = [-2.7576, 2.7576] + # hip_yaw_joint.armature = 0.01017752004 + + # # Hip yaw capsule geometry (oriented along Z for yaw movement) + # hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + # hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + # hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130205337.py b/.history/src/metamorphosis/builder/biped_20260130205337.py new file mode 100644 index 0000000..e1bc39a --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130205337.py @@ -0,0 +1,520 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + # hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + # hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + # hip_yaw.mass = param.hip_mass + # hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + # hip_yaw_joint = hip_yaw.add_joint( + # name=f"{side}_hip_yaw_joint", + # type=mujoco.mjtJoint.mjJNT_HINGE + # ) + # hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + # hip_yaw_joint.range = [-2.7576, 2.7576] + # hip_yaw_joint.armature = 0.01017752004 + + # # Hip yaw capsule geometry (oriented along Z for yaw movement) + # hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + # hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + # hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + + # Calf inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130205515.py b/.history/src/metamorphosis/builder/biped_20260130205515.py new file mode 100644 index 0000000..8b4a9cf --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130205515.py @@ -0,0 +1,519 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260130205901.py b/.history/src/metamorphosis/builder/biped_20260130205901.py new file mode 100644 index 0000000..b1e0d1c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260130205901.py @@ -0,0 +1,519 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) - First Capsule --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Capsule inertia (simplified) + cap_ixx = param.hip_mass * (3 * param.hip_capsule_radius**2 + param.hip_capsule_length**2) / 12 + cap_iyy = param.hip_mass * param.hip_capsule_radius**2 / 2 + cap_izz = cap_ixx + hip_pitch.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch capsule geometry (oriented along X for pitch movement) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_capsule_radius, 0, 0] + hip_pitch_geom.fromto = [-param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0, 0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) - Second Capsule --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_roll.mass = param.hip_mass + hip_roll.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll capsule geometry (oriented along Y for roll movement) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_capsule_radius, 0, 0] + hip_roll_geom.fromto = [0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2, 0] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - Third Capsule --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip_capsule_length/2] # Offset downward + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [cap_ixx, cap_iyy, cap_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw capsule geometry (oriented along Z for yaw movement) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_capsule_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, -param.hip_capsule_length/2, 0, 0, param.hip_capsule_length/2] + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131005405.py b/.history/src/metamorphosis/builder/biped_20260131005405.py new file mode 100644 index 0000000..2d329d9 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131005405.py @@ -0,0 +1,453 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and 3-segment capsule hips.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions (3-segment capsules) + hip_spacing: float # 左右髋关节间距 + hip_capsule_radius: float # 髋关节胶囊半径 + hip_capsule_length: float # 每个髋关节胶囊长度 + hip_mass: float # 每个髋关节胶囊质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131005625.py b/.history/src/metamorphosis/builder/biped_20260131005625.py new file mode 100644 index 0000000..75f1592 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131005625.py @@ -0,0 +1,460 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_capsule_range: tuple[float, float] = (0.03, 0.06), # For radius and length + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_capsule_range = hip_capsule_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131005637.py b/.history/src/metamorphosis/builder/biped_20260131005637.py new file mode 100644 index 0000000..8402cae --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131005637.py @@ -0,0 +1,466 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and capsule dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + hip_capsule_radius = random.uniform(*self.hip_capsule_range) + hip_capsule_length = random.uniform(*self.hip_capsule_range) + hip_mass = random.uniform(0.6, 1.2) # Each hip capsule + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131005645.py b/.history/src/metamorphosis/builder/biped_20260131005645.py new file mode 100644 index 0000000..cb9fb0b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131005645.py @@ -0,0 +1,473 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip_capsule_radius=hip_capsule_radius, + hip_capsule_length=hip_capsule_length, + hip_mass=hip_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131005653.py b/.history/src/metamorphosis/builder/biped_20260131005653.py new file mode 100644 index 0000000..fb44874 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131005653.py @@ -0,0 +1,476 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_capsule_length/2)] # Below hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + pelvis.inertia = [pelvis_inertia, pelvis_inertia, pelvis_inertia] + + # Pelvis geometry - sphere + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + pelvis_geom.size = [param.pelvis_radius, 0, 0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_radius + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip_mass + # Ellipsoid inertia (simplified) + hip_ixx = param.hip_mass * (param.hip_width**2 + param.hip_height**2) / 5 + hip_iyy = param.hip_mass * (param.hip_length**2 + param.hip_height**2) / 5 + hip_izz = param.hip_mass * (param.hip_length**2 + param.hip_width**2) / 5 + hip_pitch.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_pitch_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_pitch_geom.fromto = [-param.hip_length/2, 0, 0, param.hip_length/2, 0, 0] # Length along X + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, 0] # Same position as pitch + hip_roll.mass = param.hip_mass + hip_roll.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_roll_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_roll_geom.fromto = [0, -param.hip_length/2, 0, 0, param.hip_length/2, 0] # Length along Y + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, 0] # Same position + hip_yaw.mass = param.hip_mass + hip_yaw.inertia = [hip_ixx, hip_iyy, hip_izz] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip ellipsoid geometry -> use capsule instead (USD compatible) + hip_yaw_geom = hip_yaw.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.hip_width/4, 0, 0] # Use width as radius + hip_yaw_geom.fromto = [0, 0, -param.hip_height/2, 0, 0, param.hip_height/2] # Length along Z + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -(param.hip_height/2)] # Below hip + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin geometry - capsule + shin_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + shin_geom.size = [param.shin_radius, 0, 0] + shin_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131005819.py b/.history/src/metamorphosis/builder/biped_20260131005819.py new file mode 100644 index 0000000..28ae69d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131005819.py @@ -0,0 +1,380 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped consists of: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Hip yaw joint (virtual body for rotation around Z) + - Hip roll joint (virtual body for rotation around X) + - Hip pitch joint (thigh body, rotation around Y) + - Knee joint (shin body) + - Ankle pitch joint (virtual body) + - Ankle roll joint (foot body) + + This design uses "virtual bodies" (bodies with minimal mass and no geometry) + to achieve multi-DOF joints while respecting the one-joint-per-body constraint + of the USD converter. + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip2_length] # Below hip2 + hip_yaw.mass = 0.3 # Small intermediate mass for yaw joint + hip_yaw.inertia = [0.01, 0.01, 0.01] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] # Same position as hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131011123.py b/.history/src/metamorphosis/builder/biped_20260131011123.py new file mode 100644 index 0000000..8851295 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131011123.py @@ -0,0 +1,382 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- Hip Joint 3: YAW (rotation around Z-axis) --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip2_length] # Below hip2 + hip_yaw.mass = 0.3 # Small intermediate mass for yaw joint + hip_yaw.inertia = [0.01, 0.01, 0.01] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] # Same position as hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131014702.py b/.history/src/metamorphosis/builder/biped_20260131014702.py new file mode 100644 index 0000000..146b756 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131014702.py @@ -0,0 +1,377 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- Hip Joint 3: YAW (rotation around Z-axis) directly to THIGH --- + hip_yaw_joint = hip_roll.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # --- THIGH (connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131014817.py b/.history/src/metamorphosis/builder/biped_20260131014817.py new file mode 100644 index 0000000..51b4eb9 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131014817.py @@ -0,0 +1,382 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- Hip Joint 3: YAW (rotation around Z-axis) - separate body --- + hip_yaw = hip_roll.add_body(name=f"{side}_hip_yaw") + hip_yaw.pos = [0, 0, -param.hip2_length] # Below hip2 + hip_yaw.mass = 0.3 # Small intermediate mass for yaw joint + hip_yaw.inertia = [0.01, 0.01, 0.01] + + hip_yaw_joint = hip_yaw.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # --- THIGH (connected to hip yaw) --- + thigh = hip_yaw.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, 0] # Same position as hip yaw + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131020109.py b/.history/src/metamorphosis/builder/biped_20260131020109.py new file mode 100644 index 0000000..ce4ec01 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131020109.py @@ -0,0 +1,349 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- THIGH (connected directly to hip_pitch via yaw joint) --- + thigh = hip_pitch.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip1_length] # Below hip1 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131020556.py b/.history/src/metamorphosis/builder/biped_20260131020556.py new file mode 100644 index 0000000..7dda65f --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131020556.py @@ -0,0 +1,376 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH (connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131024435.py b/.history/src/metamorphosis/builder/biped_20260131024435.py new file mode 100644 index 0000000..7dda65f --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131024435.py @@ -0,0 +1,376 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH (connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131024438.py b/.history/src/metamorphosis/builder/biped_20260131024438.py new file mode 100644 index 0000000..7dda65f --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131024438.py @@ -0,0 +1,376 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH (connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131123554.py b/.history/src/metamorphosis/builder/biped_20260131123554.py new file mode 100644 index 0000000..929c60e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131123554.py @@ -0,0 +1,376 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131130127.py b/.history/src/metamorphosis/builder/biped_20260131130127.py new file mode 100644 index 0000000..41469e0 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131130127.py @@ -0,0 +1,380 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ TORSO_LINK (root body - box) ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Torso geometry - box + torso_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ PELVIS CONNECTION ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_link") + hip_pitch_link.pos = hip_pitch_pos + hip_pitch_link.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_link.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch geometry - capsule (cylinder) + hip_pitch_geom = hip_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip1_radius, 0, 0] + hip_pitch_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_link") + hip_roll_link.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll_link.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_link.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll geometry - capsule (cylinder) + hip_roll_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip2_radius, 0, 0] + hip_roll_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- HIP YAW LINK (connected directly to hip_roll via yaw joint) --- + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -param.hip2_length] # Below hip2 + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw geometry - capsule + hip_yaw_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.thigh_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -param.thigh_length] + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_geom.size = [param.shin_radius, 0, 0] + knee_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -param.shin_length] + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # Ankle pitch geometry - sphere + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [0.02, 0, 0] # Small sphere radius + + # --- ANKLE JOINT 2: ROLL (ANKLE_ROLL_LINK) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, 0] # Same position + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Ankle roll geometry - box extending forward + ankle_roll_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + ankle_roll_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + ankle_roll_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131131614.py b/.history/src/metamorphosis/builder/biped_20260131131614.py new file mode 100644 index 0000000..e05b3e2 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131131614.py @@ -0,0 +1,380 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ TORSO_LINK (root body - box) ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Torso geometry - box + torso_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ PELVIS CONNECTION ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_link = pelvis.add_body(name=f"{side}_hip_pitch_Link") + hip_pitch_link.pos = hip_pitch_pos + hip_pitch_link.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_link.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch_link.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip pitch geometry - capsule (cylinder) + hip_pitch_geom = hip_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.size = [param.hip1_radius, 0, 0] + hip_pitch_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll_link = hip_pitch_link.add_body(name=f"{side}_hip_roll_Link") + hip_roll_link.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll_link.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_link.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll_link.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip roll geometry - capsule (cylinder) + hip_roll_geom = hip_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.size = [param.hip2_radius, 0, 0] + hip_roll_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- HIP YAW LINK (connected directly to hip_roll via yaw joint) --- + hip_yaw_link = hip_roll_link.add_body(name=f"{side}_hip_yaw_Link") + hip_yaw_link.pos = [0, 0, -param.hip2_length] # Below hip2 + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Hip yaw geometry - capsule + hip_yaw_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.size = [param.thigh_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_Link") + knee_link.pos = [0, 0, -param.thigh_length] + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_geom.size = [param.shin_radius, 0, 0] + knee_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_Link") + ankle_pitch_link.pos = [0, 0, -param.shin_length] + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # Ankle pitch geometry - sphere + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [0.02, 0, 0] # Small sphere radius + + # --- ANKLE JOINT 2: ROLL (ANKLE_ROLL_LINK) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_Link") + ankle_roll_link.pos = [0, 0, 0] # Same position + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Ankle roll geometry - box extending forward + ankle_roll_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + ankle_roll_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + ankle_roll_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131131713.py b/.history/src/metamorphosis/builder/biped_20260131131713.py new file mode 100644 index 0000000..929c60e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131131713.py @@ -0,0 +1,376 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131131919.py b/.history/src/metamorphosis/builder/biped_20260131131919.py new file mode 100644 index 0000000..929c60e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131131919.py @@ -0,0 +1,376 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "pelvis" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="waist") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_thigh") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_foot") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131132038.py b/.history/src/metamorphosis/builder/biped_20260131132038.py new file mode 100644 index 0000000..b7f5629 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131132038.py @@ -0,0 +1,382 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + pelvis = spec.worldbody.add_body() + pelvis.name = "torso_link" + pelvis.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + pelvis.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + waist = pelvis.add_body(name="pelvis") + waist.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + waist.mass = 0.5 # Small mass for connection + waist.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = waist.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch = waist.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch.pos = hip_pitch_pos + hip_pitch.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint = hip_pitch.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint.range = [-2.5307, 2.8798] + hip_pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip1_geom = hip_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip1_geom.name = f"{side}_hip_pitch_link" + hip1_geom.size = [param.hip1_radius, 0, 0] + hip1_geom.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll = hip_pitch.add_body(name=f"{side}_hip_roll_joint") + hip_roll.pos = [0, 0, -param.hip1_length] # Below hip1 + hip_roll.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint = hip_roll.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint.range = [-2.9671, 0.5236] + hip_roll_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip2_geom = hip_roll.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip2_geom.name = f"{side}_hip_roll_link" + hip2_geom.size = [param.hip2_radius, 0, 0] + hip2_geom.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + thigh = hip_roll.add_body(name=f"{side}_hip_yaw_link") + thigh.pos = [0, 0, -param.hip2_length] # Below hip2 + thigh.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + thigh.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_joint = thigh.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_joint.range = [-2.7576, 2.7576] + hip_yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = thigh.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee = thigh.add_body(name=f"{side}_knee_link") + knee.pos = [0, 0, -param.thigh_length] + knee.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + calf_geom = knee.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + calf_geom.name = f"{side}_knee_link" + calf_geom.size = [param.shin_radius, 0, 0] + calf_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch = knee.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch.pos = [0, 0, -param.shin_length] + ankle_pitch.mass = 0.3 # Small intermediate mass + ankle_pitch.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [min(param.foot_width, param.foot_height) / 2, 0, 0] + + ankle_pitch_joint = ankle_pitch.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + foot = ankle_pitch.add_body(name=f"{side}_ankle_roll_link") + foot.pos = [0, 0, 0] # Same position + foot.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + foot.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = foot.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = foot.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131132713.py b/.history/src/metamorphosis/builder/biped_20260131132713.py new file mode 100644 index 0000000..5c75c2b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131132713.py @@ -0,0 +1,382 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -param.hip1_length] # Below hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -param.hip2_length] # Below hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -param.thigh_length] + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -param.shin_length] + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [min(param.foot_width, param.foot_height) / 2, 0, 0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, 0] # Same position + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131145620.py b/.history/src/metamorphosis/builder/biped_20260131145620.py new file mode 100644 index 0000000..5c75c2b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131145620.py @@ -0,0 +1,382 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ PELVIS (root body - box) ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -param.hip1_length] # Below hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -param.hip2_length] # Below hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -param.thigh_length] + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -param.shin_length] + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [min(param.foot_width, param.foot_height) / 2, 0, 0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, 0] # Same position + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131150938.py b/.history/src/metamorphosis/builder/biped_20260131150938.py new file mode 100644 index 0000000..4032df4 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131150938.py @@ -0,0 +1,392 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Joint 1: PITCH (rotation around Y-axis) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [0, 1, 0] # Y-axis (pitch) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Joint 2: ROLL (rotation around X-axis) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- THIGH Joint(connected directly to hip_roll via yaw joint) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131160757.py b/.history/src/metamorphosis/builder/biped_20260131160757.py new file mode 100644 index 0000000..2144590 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131160757.py @@ -0,0 +1,392 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131160928.py b/.history/src/metamorphosis/builder/biped_20260131160928.py new file mode 100644 index 0000000..ace705a --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131160928.py @@ -0,0 +1,392 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131161243.py b/.history/src/metamorphosis/builder/biped_20260131161243.py new file mode 100644 index 0000000..3e64990 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131161243.py @@ -0,0 +1,402 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.qpos0 = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131161508.py b/.history/src/metamorphosis/builder/biped_20260131161508.py new file mode 100644 index 0000000..0b7861d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131161508.py @@ -0,0 +1,408 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131173115.py b/.history/src/metamorphosis/builder/biped_20260131173115.py new file mode 100644 index 0000000..0b7861d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131173115.py @@ -0,0 +1,408 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131173326.py b/.history/src/metamorphosis/builder/biped_20260131173326.py new file mode 100644 index 0000000..0b7861d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131173326.py @@ -0,0 +1,408 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131173357.py b/.history/src/metamorphosis/builder/biped_20260131173357.py new file mode 100644 index 0000000..9a3ab71 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131173357.py @@ -0,0 +1,408 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131173813.py b/.history/src/metamorphosis/builder/biped_20260131173813.py new file mode 100644 index 0000000..8ad9d12 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131173813.py @@ -0,0 +1,408 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131174415.py b/.history/src/metamorphosis/builder/biped_20260131174415.py new file mode 100644 index 0000000..afab100 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131174415.py @@ -0,0 +1,414 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131174908.py b/.history/src/metamorphosis/builder/biped_20260131174908.py new file mode 100644 index 0000000..afab100 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131174908.py @@ -0,0 +1,414 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Pelvis (main body - box) + pelvis_length: float # 长方体骨盆长度 (X方向) + pelvis_width: float # 长方体骨盆宽度 (Y方向) + pelvis_height: float # 长方体骨盆高度 (Z方向) + pelvis_mass: float # 骨盆质量 + + # Waist connection + waist_height: float # 腰部连接高度 + waist_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = hip_pitch_pos + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131180210.py b/.history/src/metamorphosis/builder/biped_20260131180210.py new file mode 100644 index 0000000..62dcf5b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131180210.py @@ -0,0 +1,414 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_pos = + + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131180215.py b/.history/src/metamorphosis/builder/biped_20260131180215.py new file mode 100644 index 0000000..6b89503 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131180215.py @@ -0,0 +1,412 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131180217.py b/.history/src/metamorphosis/builder/biped_20260131180217.py new file mode 100644 index 0000000..6b89503 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131180217.py @@ -0,0 +1,412 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip1 parameters (first hip segment) + hip1_length: float # 髋关节1长度 + hip1_radius: float # 髋关节1半径 + hip1_mass: float # 髋关节1质量 + + # Hip2 parameters (second hip segment) + hip2_length: float # 髋关节2长度 + hip2_radius: float # 髋关节2半径 + hip2_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.waist_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131180645.py b/.history/src/metamorphosis/builder/biped_20260131180645.py new file mode 100644 index 0000000..6797e76 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131180645.py @@ -0,0 +1,412 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # Thigh parameters + thigh_length: float # 大腿长度 + thigh_radius: float # 大腿粗细 + thigh_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131180912.py b/.history/src/metamorphosis/builder/biped_20260131180912.py new file mode 100644 index 0000000..8989f1b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131180912.py @@ -0,0 +1,412 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # Shin parameters + shin_length: float # 小腿长度 + shin_radius: float # 小腿粗细 + shin_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131180934.py b/.history/src/metamorphosis/builder/biped_20260131180934.py new file mode 100644 index 0000000..75caaac --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131180934.py @@ -0,0 +1,412 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # Foot parameters + foot_length: float # 脚长 + foot_width: float # 脚宽 + foot_height: float # 脚厚度 + foot_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131181124.py b/.history/src/metamorphosis/builder/biped_20260131181124.py new file mode 100644 index 0000000..c42f8ee --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131181124.py @@ -0,0 +1,417 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_roll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + pelvis_length_range: tuple[float, float] = (0.10, 0.16), + pelvis_width_range: tuple[float, float] = (0.18, 0.26), + pelvis_height_range: tuple[float, float] = (0.08, 2.0), + waist_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip1_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip1_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip2_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip2_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131181812.py b/.history/src/metamorphosis/builder/biped_20260131181812.py new file mode 100644 index 0000000..898a3ea --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131181812.py @@ -0,0 +1,417 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_roll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131181816.py b/.history/src/metamorphosis/builder/biped_20260131181816.py new file mode 100644 index 0000000..92b4bad --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131181816.py @@ -0,0 +1,417 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.pelvis_length_range = pelvis_length_range + self.pelvis_width_range = pelvis_width_range + self.pelvis_height_range = pelvis_height_range + self.waist_height_range = waist_height_range + self.hip_spacing_range = hip_spacing_range + self.hip1_length_range = hip1_length_range + self.hip1_radius_range = hip1_radius_range + self.hip2_length_range = hip2_length_range + self.hip2_radius_range = hip2_radius_range + self.hip_pitch_roll_range = hip_pitch_roll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131185738.py b/.history/src/metamorphosis/builder/biped_20260131185738.py new file mode 100644 index 0000000..1d3b68e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131185738.py @@ -0,0 +1,417 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Pelvis (box) + pelvis_length = random.uniform(*self.pelvis_length_range) + pelvis_width = random.uniform(*self.pelvis_width_range) + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_mass = random.uniform(3.0, 5.0) + + # Waist connection + waist_height = random.uniform(*self.waist_height_range) + waist_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131192019.py b/.history/src/metamorphosis/builder/biped_20260131192019.py new file mode 100644 index 0000000..9a50588 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131192019.py @@ -0,0 +1,417 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Pelvis connection + pelvis_height = random.uniform(*self.waist_height_range) + pelvis_radius = min(pelvis_width, pelvis_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131192406.py b/.history/src/metamorphosis/builder/biped_20260131192406.py new file mode 100644 index 0000000..b03d396 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131192406.py @@ -0,0 +1,417 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Pelvis connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip1 dimensions (first hip segment) + hip1_length = random.uniform(*self.hip1_length_range) + hip1_radius = random.uniform(*self.hip1_radius_range) + hip1_mass = random.uniform(0.4, 0.8) # Hip1 mass + + # Hip2 dimensions (second hip segment) + hip2_length = random.uniform(*self.hip2_length_range) + hip2_radius = random.uniform(*self.hip2_radius_range) + hip2_mass = random.uniform(0.4, 0.8) # Hip2 mass + + # Hip pitch roll initialization (0 to 90 degrees) + hip_pitch_roll = random.uniform(*self.hip_pitch_roll_range) + + # Leg dimensions + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + thigh_length = total_leg_length / (1 + shin_ratio) + shin_length = thigh_length * shin_ratio + + # Leg masses and radii (proportional to length) + thigh_radius = random.uniform(0.025, 0.040) + thigh_mass = thigh_length * random.uniform(1.5, 2.2) + + shin_radius = thigh_radius * random.uniform(0.75, 0.95) + shin_mass = shin_length * random.uniform(1.2, 1.8) + + # Foot parameters + foot_length = random.uniform(0.18, 0.25) + foot_width = random.uniform(0.06, 0.10) + foot_height = random.uniform(0.02, 0.03) + foot_mass = random.uniform(0.2, 0.5) + + param = BipedParam( + pelvis_length=pelvis_length, + pelvis_width=pelvis_width, + pelvis_height=pelvis_height, + pelvis_mass=pelvis_mass, + waist_height=waist_height, + waist_radius=waist_radius, + hip_spacing=hip_spacing, + hip1_length=hip1_length, + hip1_radius=hip1_radius, + hip1_mass=hip1_mass, + hip2_length=hip2_length, + hip2_radius=hip2_radius, + hip2_mass=hip2_mass, + hip_pitch_roll=hip_pitch_roll, + thigh_length=thigh_length, + thigh_radius=thigh_radius, + thigh_mass=thigh_mass, + shin_length=shin_length, + shin_radius=shin_radius, + shin_mass=shin_mass, + foot_length=foot_length, + foot_width=foot_width, + foot_height=foot_height, + foot_mass=foot_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131200507.py b/.history/src/metamorphosis/builder/biped_20260131200507.py new file mode 100644 index 0000000..6941a59 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131200507.py @@ -0,0 +1,425 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.pelvis_mass + # Box inertia + ixx = param.pelvis_mass * (param.pelvis_width**2 + param.pelvis_height**2) / 12 + iyy = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_height**2) / 12 + izz = param.pelvis_mass * (param.pelvis_length**2 + param.pelvis_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.pelvis_length/2, param.pelvis_width/2, param.pelvis_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.pelvis_height/2 + param.waist_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.waist_radius, param.waist_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_roll if side == "left" else -param.hip_pitch_roll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip1_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip1_mass * (3 * param.hip1_radius**2 + param.hip1_length**2) / 12 + hip1_iyy = param.hip1_mass * param.hip1_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip1_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip1_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip1_length + param.hip1_radius + param.hip2_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip2_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip2_mass * (3 * param.hip2_radius**2 + param.hip2_length**2) / 12 + hip2_iyy = param.hip2_mass * param.hip2_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip2_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip2_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip2_length + param.hip2_radius + param.thigh_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.thigh_mass + # Cylinder inertia + thigh_ixx = param.thigh_mass * (3 * param.thigh_radius**2 + param.thigh_length**2) / 12 + thigh_iyy = param.thigh_mass * param.thigh_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.thigh_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.thigh_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.thigh_length + param.thigh_radius + param.shin_radius)] # Tangent to thigh + knee_link.mass = param.shin_mass + # Shin inertia + shin_ixx = param.shin_mass * (3 * param.shin_radius**2 + param.shin_length**2) / 12 + shin_iyy = param.shin_mass * param.shin_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.shin_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.shin_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_radius = min(param.foot_width, param.foot_height) / 2 + + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.shin_length + param.shin_radius + ankle_pitch_radius)] # Tangent to knee_link + ankle_pitch_link.mass = 0.3 # Small intermediate mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [ankle_pitch_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(ankle_pitch_radius + param.foot_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.foot_mass + # Foot inertia + foot_ixx = param.foot_mass * (param.foot_width**2 + param.foot_height**2) / 12 + foot_iyy = param.foot_mass * (param.foot_length**2 + param.foot_height**2) / 12 + foot_izz = param.foot_mass * (param.foot_length**2 + param.foot_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.foot_length/2, param.foot_width/2, param.foot_height/2] + foot_geom.pos = [param.foot_length/4, 0, -param.foot_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131200533.py b/.history/src/metamorphosis/builder/biped_20260131200533.py new file mode 100644 index 0000000..563ee9f --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131200533.py @@ -0,0 +1,423 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + pelvis_height_range: Tuple of (min, max) for pelvis height in meters. + pelvis_width_range: Tuple of (min, max) for pelvis width in meters. + leg_length_range: Tuple of (min, max) for total leg length in meters. + shin_ratio_range: Tuple of (min, max) for shin/thigh length ratio. + hip1_length_range: Tuple of (min, max) for hip1 segment length. + hip2_length_range: Tuple of (min, max) for hip2 segment length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131200559.py b/.history/src/metamorphosis/builder/biped_20260131200559.py new file mode 100644 index 0000000..0793e71 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131200559.py @@ -0,0 +1,425 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ WAIST CONNECTION ============ + # Small cylindrical waist below pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Waist geometry - small cylinder + waist_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + waist_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + waist_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131203008.py b/.history/src/metamorphosis/builder/biped_20260131203008.py new file mode 100644 index 0000000..56946d6 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131203008.py @@ -0,0 +1,425 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.08, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204040.py b/.history/src/metamorphosis/builder/biped_20260131204040.py new file mode 100644 index 0000000..e8f0f51 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204040.py @@ -0,0 +1,425 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float # 长方体骨盆长度 (X方向) + torso_width: float # 长方体骨盆宽度 (Y方向) + torso_height: float # 长方体骨盆高度 (Z方向) + torso_mass: float # 骨盆质量 + + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204506.py b/.history/src/metamorphosis/builder/biped_20260131204506.py new file mode 100644 index 0000000..9d2589d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204506.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> + hip joint3 -> thigh -> knee joint -> calf -> ankle joints -> foot + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204632.py b/.history/src/metamorphosis/builder/biped_20260131204632.py new file mode 100644 index 0000000..726ba81 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204632.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_link -> ankle_roll_joint -> + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204705.py b/.history/src/metamorphosis/builder/biped_20260131204705.py new file mode 100644 index 0000000..7a4566b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204705.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204712.py b/.history/src/metamorphosis/builder/biped_20260131204712.py new file mode 100644 index 0000000..9681097 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204712.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204726.py b/.history/src/metamorphosis/builder/biped_20260131204726.py new file mode 100644 index 0000000..9681097 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204726.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131204738.py b/.history/src/metamorphosis/builder/biped_20260131204738.py new file mode 100644 index 0000000..9681097 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131204738.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度（虚拟体） + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131210341.py b/.history/src/metamorphosis/builder/biped_20260131210341.py new file mode 100644 index 0000000..b80ad5c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131210341.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip1_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip1_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip1_izz = hip1_ixx + hip_pitch_joint.inertia = [hip1_ixx, hip1_iyy, hip1_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211243.py b/.history/src/metamorphosis/builder/biped_20260131211243.py new file mode 100644 index 0000000..3ece9e7 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211243.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip2_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip2_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip2_izz = hip2_ixx + hip_roll_joint.inertia = [hip2_ixx, hip2_iyy, hip2_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211316.py b/.history/src/metamorphosis/builder/biped_20260131211316.py new file mode 100644 index 0000000..e81546f --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211316.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # Hip2 geometry - capsule (cylinder) + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211349.py b/.history/src/metamorphosis/builder/biped_20260131211349.py new file mode 100644 index 0000000..59626e2 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211349.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + thigh_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + thigh_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + thigh_izz = thigh_ixx + hip_yaw_link.inertia = [thigh_ixx, thigh_iyy, thigh_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211430.py b/.history/src/metamorphosis/builder/biped_20260131211430.py new file mode 100644 index 0000000..ce5b28a --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211430.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_hoint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + thigh_geom = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + thigh_geom.size = [param.hip_yaw_link_radius, 0, 0] + thigh_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + thigh_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT (pitch only) --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211638.py b/.history/src/metamorphosis/builder/biped_20260131211638.py new file mode 100644 index 0000000..8dfcd39 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211638.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + shin_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211652.py b/.history/src/metamorphosis/builder/biped_20260131211652.py new file mode 100644 index 0000000..184d296 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211652.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + shin_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + shin_izz = shin_ixx + knee_link.inertia = [shin_ixx, shin_iyy, shin_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211654.py b/.history/src/metamorphosis/builder/biped_20260131211654.py new file mode 100644 index 0000000..ae17f16 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211654.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Calf geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211703.py b/.history/src/metamorphosis/builder/biped_20260131211703.py new file mode 100644 index 0000000..42f12f9 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211703.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_geom = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131211942.py b/.history/src/metamorphosis/builder/biped_20260131211942.py new file mode 100644 index 0000000..f458007 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131211942.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry - small cylinder + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131212015.py b/.history/src/metamorphosis/builder/biped_20260131212015.py new file mode 100644 index 0000000..9030540 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131212015.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Pelvis geometry - box + pelvis_geom = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + pelvis_geom.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + pelvis_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131212621.py b/.history/src/metamorphosis/builder/biped_20260131212621.py new file mode 100644 index 0000000..917b484 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131212621.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_length: float + torso_width: float + torso_height: float + torso_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + + # Torso + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213033.py b/.history/src/metamorphosis/builder/biped_20260131213033.py new file mode 100644 index 0000000..b0030e7 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213033.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_length_range: tuple[float, float] = (0.10, 0.16), + torso_width_range: tuple[float, float] = (0.18, 0.26), + torso_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + # Torso + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213044.py b/.history/src/metamorphosis/builder/biped_20260131213044.py new file mode 100644 index 0000000..a7411dd --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213044.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_length_range = torso_length_range + self.torso_width_range = torso_width_range + self.torso_height_range = torso_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + # Torso + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213108.py b/.history/src/metamorphosis/builder/biped_20260131213108.py new file mode 100644 index 0000000..fcae650 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213108.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_length = random.uniform(*self.torso_length_range) + torso_width = random.uniform(*self.torso_width_range) + torso_height = random.uniform(*self.torso_height_range) + torso_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + # Torso + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213151.py b/.history/src/metamorphosis/builder/biped_20260131213151.py new file mode 100644 index 0000000..398203d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213151.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_width, torso_length) * random.uniform(0.2, 0.4) + + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_mass + # Box inertia + ixx = param.torso_mass * (param.torso_width**2 + param.torso_height**2) / 12 + iyy = param.torso_mass * (param.torso_length**2 + param.torso_height**2) / 12 + izz = param.torso_mass * (param.torso_length**2 + param.torso_width**2) / 12 + torso_link.inertia = [ixx, iyy, izz] + # Torso + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_length/2, param.torso_width/2, param.torso_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213338.py b/.history/src/metamorphosis/builder/biped_20260131213338.py new file mode 100644 index 0000000..6cc1242 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213338.py @@ -0,0 +1,421 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_link_mass + # Box inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213556.py b/.history/src/metamorphosis/builder/biped_20260131213556.py new file mode 100644 index 0000000..1299b2e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213556.py @@ -0,0 +1,421 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Small cylindrical pelvis below torso + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 # Small mass for connection + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213657.py b/.history/src/metamorphosis/builder/biped_20260131213657.py new file mode 100644 index 0000000..62668ed --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213657.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213704.py b/.history/src/metamorphosis/builder/biped_20260131213704.py new file mode 100644 index 0000000..a9f6ea2 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213704.py @@ -0,0 +1,423 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body() + torso_link.name = "torso_link" + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131213744.py b/.history/src/metamorphosis/builder/biped_20260131213744.py new file mode 100644 index 0000000..15becb1 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131213744.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip Pitch Joint Body (joint: hip_pitch_joint, link: hip_pitch_link) --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Cylinder inertia for hip1 + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131214305.py b/.history/src/metamorphosis/builder/biped_20260131214305.py new file mode 100644 index 0000000..6f9f7e5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131214305.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + hip_pitch_joint_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_pitch_joint_joint.axis = [1, 0, 0] # X-axis (roll) + hip_pitch_joint_joint.range = [-2.5307, 2.8798] + hip_pitch_joint_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131215228.py b/.history/src/metamorphosis/builder/biped_20260131215228.py new file mode 100644 index 0000000..72b2492 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131215228.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + hip_roll_joint_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_roll_joint_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + hip_roll_joint_joint.range = [-0.5236, 2.9671] + else: + hip_roll_joint_joint.range = [-2.9671, 0.5236] + hip_roll_joint_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131215233.py b/.history/src/metamorphosis/builder/biped_20260131215233.py new file mode 100644 index 0000000..93b9aaf --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131215233.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + hip_yaw_hoint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + hip_yaw_hoint.axis = [0, 0, 1] # Z-axis (yaw) + hip_yaw_hoint.range = [-2.7576, 2.7576] + hip_yaw_hoint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131215341.py b/.history/src/metamorphosis/builder/biped_20260131215341.py new file mode 100644 index 0000000..fcccae1 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131215341.py @@ -0,0 +1,422 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131215519.py b/.history/src/metamorphosis/builder/biped_20260131215519.py new file mode 100644 index 0000000..4d2c0dd --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131215519.py @@ -0,0 +1,423 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220432.py b/.history/src/metamorphosis/builder/biped_20260131220432.py new file mode 100644 index 0000000..913a6dd --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220432.py @@ -0,0 +1,424 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220439.py b/.history/src/metamorphosis/builder/biped_20260131220439.py new file mode 100644 index 0000000..bc73113 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220439.py @@ -0,0 +1,425 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220445.py b/.history/src/metamorphosis/builder/biped_20260131220445.py new file mode 100644 index 0000000..458a89b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220445.py @@ -0,0 +1,426 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220450.py b/.history/src/metamorphosis/builder/biped_20260131220450.py new file mode 100644 index 0000000..f845e1c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220450.py @@ -0,0 +1,427 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220458.py b/.history/src/metamorphosis/builder/biped_20260131220458.py new file mode 100644 index 0000000..7672404 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220458.py @@ -0,0 +1,428 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220503.py b/.history/src/metamorphosis/builder/biped_20260131220503.py new file mode 100644 index 0000000..7672404 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220503.py @@ -0,0 +1,428 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- Hip_pitch_link --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220623.py b/.history/src/metamorphosis/builder/biped_20260131220623.py new file mode 100644 index 0000000..7dd440d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220623.py @@ -0,0 +1,428 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # Hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220646.py b/.history/src/metamorphosis/builder/biped_20260131220646.py new file mode 100644 index 0000000..0a44d54 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220646.py @@ -0,0 +1,429 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + + # Hip1 geometry - capsule (cylinder) + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220712.py b/.history/src/metamorphosis/builder/biped_20260131220712.py new file mode 100644 index 0000000..07c1992 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220712.py @@ -0,0 +1,428 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- Hip Roll Joint Body (joint: hip_roll_joint, link: hip_roll_link) --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220723.py b/.history/src/metamorphosis/builder/biped_20260131220723.py new file mode 100644 index 0000000..ec97277 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220723.py @@ -0,0 +1,428 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220732.py b/.history/src/metamorphosis/builder/biped_20260131220732.py new file mode 100644 index 0000000..41487f4 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220732.py @@ -0,0 +1,429 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220756.py b/.history/src/metamorphosis/builder/biped_20260131220756.py new file mode 100644 index 0000000..83be2ae --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220756.py @@ -0,0 +1,430 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220821.py b/.history/src/metamorphosis/builder/biped_20260131220821.py new file mode 100644 index 0000000..1e76ee7 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220821.py @@ -0,0 +1,430 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220837.py b/.history/src/metamorphosis/builder/biped_20260131220837.py new file mode 100644 index 0000000..02713e4 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220837.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Cylinder inertia for hip2 + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220849.py b/.history/src/metamorphosis/builder/biped_20260131220849.py new file mode 100644 index 0000000..0dfed62 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220849.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220854.py b/.history/src/metamorphosis/builder/biped_20260131220854.py new file mode 100644 index 0000000..7cada1c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220854.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + + # hip_roll_link + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220908.py b/.history/src/metamorphosis/builder/biped_20260131220908.py new file mode 100644 index 0000000..e49500d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220908.py @@ -0,0 +1,430 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- Hip Yaw Joint Body (joint: hip_yaw_hoint, link: hip_yaw_link) --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220926.py b/.history/src/metamorphosis/builder/biped_20260131220926.py new file mode 100644 index 0000000..b67fcab --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220926.py @@ -0,0 +1,430 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220930.py b/.history/src/metamorphosis/builder/biped_20260131220930.py new file mode 100644 index 0000000..6fbca19 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220930.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220935.py b/.history/src/metamorphosis/builder/biped_20260131220935.py new file mode 100644 index 0000000..3f5cf19 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220935.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # Cylinder inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220947.py b/.history/src/metamorphosis/builder/biped_20260131220947.py new file mode 100644 index 0000000..7fef59d --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220947.py @@ -0,0 +1,432 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + + # Thigh geometry - capsule + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220956.py b/.history/src/metamorphosis/builder/biped_20260131220956.py new file mode 100644 index 0000000..aa708b5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220956.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131220957.py b/.history/src/metamorphosis/builder/biped_20260131220957.py new file mode 100644 index 0000000..aa708b5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131220957.py @@ -0,0 +1,431 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131221033.py b/.history/src/metamorphosis/builder/biped_20260131221033.py new file mode 100644 index 0000000..894d180 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131221033.py @@ -0,0 +1,432 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- KNEE JOINT --- + # Add knee_link body + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131221120.py b/.history/src/metamorphosis/builder/biped_20260131221120.py new file mode 100644 index 0000000..f5e0ab5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131221120.py @@ -0,0 +1,432 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_link = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_link.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_link.mass = param.hip_yaw_link_mass + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_link.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_ body + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131221434.py b/.history/src/metamorphosis/builder/biped_20260131221434.py new file mode 100644 index 0000000..ce98385 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131221434.py @@ -0,0 +1,434 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + # No initial rotation for yaw joint + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_ body + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131221501.py b/.history/src/metamorphosis/builder/biped_20260131221501.py new file mode 100644 index 0000000..097fadb --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131221501.py @@ -0,0 +1,433 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_ body + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222037.py b/.history/src/metamorphosis/builder/biped_20260131222037.py new file mode 100644 index 0000000..7d96c3a --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222037.py @@ -0,0 +1,434 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_ body + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222119.py b/.history/src/metamorphosis/builder/biped_20260131222119.py new file mode 100644 index 0000000..8d57682 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222119.py @@ -0,0 +1,434 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_link = hip_yaw_link.add_body(name=f"{side}_knee_link") + knee_link.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] # Tangent to thigh + knee_link.mass = param.knee_link_mass + + knee_joint = knee_link.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222243.py b/.history/src/metamorphosis/builder/biped_20260131222243.py new file mode 100644 index 0000000..786dd75 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222243.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + # No initial rotation for knee joint + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + + knee_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222354.py b/.history/src/metamorphosis/builder/biped_20260131222354.py new file mode 100644 index 0000000..3af1ef5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222354.py @@ -0,0 +1,435 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + knee_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222516.py b/.history/src/metamorphosis/builder/biped_20260131222516.py new file mode 100644 index 0000000..0f07eff --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222516.py @@ -0,0 +1,435 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_link") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + knee_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222538.py b/.history/src/metamorphosis/builder/biped_20260131222538.py new file mode 100644 index 0000000..3af1ef5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222538.py @@ -0,0 +1,435 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + knee_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222555.py b/.history/src/metamorphosis/builder/biped_20260131222555.py new file mode 100644 index 0000000..3af1ef5 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222555.py @@ -0,0 +1,435 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + knee_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + knee_joint.axis = [0, 1, 0] # Y-axis (pitch) + knee_joint.range = [-0.087267, 2.8798] + knee_joint.armature = 0.025101925 + + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131222848.py b/.history/src/metamorphosis/builder/biped_20260131222848.py new file mode 100644 index 0000000..5c6324b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131222848.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131223126.py b/.history/src/metamorphosis/builder/biped_20260131223126.py new file mode 100644 index 0000000..568e925 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131223126.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint center body + hip_pitch_joint_center = pelvis.add_body(name=f"{side}_hip_pitch_joint_center") + hip_pitch_joint_center.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint_center.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint_center.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint_center.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint_center.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint_center.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint center body + hip_roll_joint_center = hip_pitch_joint_center.add_body(name=f"{side}_hip_roll_joint_center") + hip_roll_joint_center.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint_center.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint_center.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint_center.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint_center.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint_center.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint center body + hip_yaw_joint_center = hip_roll_joint_center.add_body(name=f"{side}_hip_yaw_joint_center") + hip_yaw_joint_center.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint_center.mass = param.hip_yaw_link_mass + hip_yaw_joint_center.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint_center.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint_center.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint_center.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint center body + knee_joint_center = hip_yaw_joint_center.add_body(name=f"{side}_knee_joint_center") + knee_joint_center.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint_center.mass = param.knee_link_mass + knee_joint_center.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint_center.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_joint_center = knee_joint_center.add_body(name=f"{side}_ankle_pitch_joint_center") + ankle_pitch_joint_center.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_joint_center.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_joint_center.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_joint_center.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_joint_center.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_joint_center = ankle_pitch_joint_center.add_body(name=f"{side}_ankle_roll_joint_center") + ankle_roll_joint_center.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_joint_center.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_joint_center.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_joint_center.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_joint_center.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131224335.py b/.history/src/metamorphosis/builder/biped_20260131224335.py new file mode 100644 index 0000000..88c3241 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131224335.py @@ -0,0 +1,425 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis_body = torso_body.add_body(name="pelvis") + pelvis_body.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis_body.mass = 0.5 + # Pelvis inertia + pelvis_body.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_geom = pelvis_body.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_geom.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_geom.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + hip_pitch_body = pelvis_body.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_body.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_body.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_body.mass = param.hip_pitch_link_mass + pitch_joint = hip_pitch_body.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_body.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + hip_pitch_geom = hip_pitch_body.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_geom.name = f"{side}_hip_pitch_link" + hip_pitch_geom.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_geom.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_geom.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + hip_roll_body = hip_pitch_body.add_body(name=f"{side}_hip_roll_joint") + hip_roll_body.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_body.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_body.mass = param.hip_roll_link_mass + roll_joint = hip_roll_body.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_body.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + hip_roll_geom = hip_roll_body.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_geom.name = f"{side}_hip_roll_link" + hip_roll_geom.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_geom.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_geom.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + hip_yaw_body = hip_roll_body.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_body.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_body.mass = param.hip_yaw_link_mass + hip_yaw_body.quat = [1.0, 0.0, 0.0, 0.0] + yaw_joint = hip_yaw_body.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_body.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + hip_yaw_geom = hip_yaw_body.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + knee_body = hip_yaw_body.add_body(name=f"{side}_knee_joint") + knee_body.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_body.mass = param.knee_link_mass + knee_body.quat = [1.0, 0.0, 0.0, 0.0] + pitch_joint = knee_body.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_body.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_body.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_body = knee_body.add_body(name=f"{side}_ankle_pitch_joint") + ankle_pitch_body.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_body.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_body.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_body.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_body.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_body = ankle_pitch_body.add_body(name=f"{side}_ankle_roll_joint") + ankle_roll_body.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_body.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_body.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_body.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131225022.py b/.history/src/metamorphosis/builder/biped_20260131225022.py new file mode 100644 index 0000000..5c6324b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131225022.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131225024.py b/.history/src/metamorphosis/builder/biped_20260131225024.py new file mode 100644 index 0000000..5c6324b --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131225024.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_length_range/torso_width_range/torso_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_length=torso_length, + torso_width=torso_width, + torso_height=torso_height, + torso_mass=torso_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso + torso_link = spec.worldbody.add_body(name="torso_link") + torso_link.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_link.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_link = torso_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_link.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_link.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_link.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131225142.py b/.history/src/metamorphosis/builder/biped_20260131225142.py new file mode 100644 index 0000000..9d7c75c --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131225142.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_link.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_link = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_link.name = f"{side}_hip_yaw_link" + hip_yaw_link.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_link.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_link.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131225300.py b/.history/src/metamorphosis/builder/biped_20260131225300.py new file mode 100644 index 0000000..2942b98 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131225300.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_geom = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_link.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_link.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_link.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131225453.py b/.history/src/metamorphosis/builder/biped_20260131225453.py new file mode 100644 index 0000000..c2c2a28 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131225453.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_geom = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_joint.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_joint.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131225754.py b/.history/src/metamorphosis/builder/biped_20260131225754.py new file mode 100644 index 0000000..c2c2a28 --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131225754.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # 腰部连接高度 + pelvis_radius: float # 腰部半径（小圆柱） + + # Hip positioning and dimensions + hip_spacing: float # 左右髋关节间距 + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # 髋关节1长度 + hip_pitch_link_radius: float # 髋关节1半径 + hip_pitch_link_mass: float # 髋关节1质量 + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # 髋关节2长度 + hip_roll_link_radius: float # 髋关节2半径 + hip_roll_link_mass: float # 髋关节2质量 + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # 髋关节1绕X轴初始外扩角度 + + # hip_yaw_link parameters + hip_yaw_link_length: float # 大腿长度 + hip_yaw_link_radius: float # 大腿粗细 + hip_yaw_link_mass: float # 大腿质量 + + # knee_link parameters + knee_link_length: float # 小腿长度 + knee_link_radius: float # 小腿粗细 + knee_link_mass: float # 小腿质量 + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # 踝关节连接体长度 + ankle_pitch_link_radius: float # 踝关节连接体半径 + ankle_pitch_link_mass: float # 踝关节连接体质量 + + # ankle_roll_link parameters + ankle_roll_link_length: float # 脚长 + ankle_roll_link_width: float # 脚宽 + ankle_roll_link_height: float # 脚厚度 + ankle_roll_link_mass: float # 脚质量 + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_geom = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_joint.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_joint.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131230247.py b/.history/src/metamorphosis/builder/biped_20260131230247.py new file mode 100644 index 0000000..e86d61e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131230247.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # waist connection height + pelvis_radius: float # waist radius (small cylinder) + + # Hip positioning and dimensions + hip_spacing: float # left-right hip spacing + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # hip pitch link length + hip_pitch_link_radius: float # hip pitch link radius + hip_pitch_link_mass: float # hip pitch link mass + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # hip roll link length + hip_roll_link_radius: float # hip roll link radius + hip_roll_link_mass: float # hip roll link mass + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # initial outward roll angle of hip pitch link around X-axis + + # hip_yaw_link parameters + hip_yaw_link_length: float # thigh length + hip_yaw_link_radius: float # thigh radius + hip_yaw_link_mass: float # thigh mass + + # knee_link parameters + knee_link_length: float # shin length + knee_link_radius: float # shin radius + knee_link_mass: float # shin mass + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # ankle pitch link length + ankle_pitch_link_radius: float # ankle pitch link radius + ankle_pitch_link_mass: float # ankle pitch link mass + + # ankle_roll_link parameters + ankle_roll_link_length: float # foot length + ankle_roll_link_width: float # foot width + ankle_roll_link_height: float # foot height + ankle_roll_link_mass: float # foot mass + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_geom = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_joint.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_joint.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/src/metamorphosis/builder/biped_20260131230302.py b/.history/src/metamorphosis/builder/biped_20260131230302.py new file mode 100644 index 0000000..e86d61e --- /dev/null +++ b/.history/src/metamorphosis/builder/biped_20260131230302.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # waist connection height + pelvis_radius: float # waist radius (small cylinder) + + # Hip positioning and dimensions + hip_spacing: float # left-right hip spacing + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # hip pitch link length + hip_pitch_link_radius: float # hip pitch link radius + hip_pitch_link_mass: float # hip pitch link mass + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # hip roll link length + hip_roll_link_radius: float # hip roll link radius + hip_roll_link_mass: float # hip roll link mass + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # initial outward roll angle of hip pitch link around X-axis + + # hip_yaw_link parameters + hip_yaw_link_length: float # thigh length + hip_yaw_link_radius: float # thigh radius + hip_yaw_link_mass: float # thigh mass + + # knee_link parameters + knee_link_length: float # shin length + knee_link_radius: float # shin radius + knee_link_mass: float # shin mass + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # ankle pitch link length + ankle_pitch_link_radius: float # ankle pitch link radius + ankle_pitch_link_mass: float # ankle pitch link mass + + # ankle_roll_link parameters + ankle_roll_link_length: float # foot length + ankle_roll_link_width: float # foot width + ankle_roll_link_height: float # foot height + ankle_roll_link_mass: float # foot mass + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_geom = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_joint.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_joint.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/.history/test_biped_20260131010954.py b/.history/test_biped_20260131010954.py new file mode 100644 index 0000000..8ca637d --- /dev/null +++ b/.history/test_biped_20260131010954.py @@ -0,0 +1,43 @@ +#!/usr/bin/env python3 + +from src.metamorphosis.builder.biped import BipedBuilder + +def test_biped(): + print("Testing updated biped structure...") + + # Create builder + builder = BipedBuilder() + + # Sample parameters + param = builder.sample_params(seed=42) + + print("✓ Sample parameters generated successfully!") + print(f" Pelvis: {param.pelvis_length:.3f} × {param.pelvis_width:.3f} × {param.pelvis_height:.3f}") + print(f" Hip spacing: {param.hip_spacing:.3f}") + print(f" Hip1: radius={param.hip1_radius:.3f}, length={param.hip1_length:.3f}, mass={param.hip1_mass:.2f}") + print(f" Hip2: radius={param.hip2_radius:.3f}, length={param.hip2_length:.3f}, mass={param.hip2_mass:.2f}") + print(f" Thigh: radius={param.thigh_radius:.3f}, length={param.thigh_length:.3f}, mass={param.thigh_mass:.2f}") + print(f" Shin: radius={param.shin_radius:.3f}, length={param.shin_length:.3f}, mass={param.shin_mass:.2f}") + + # Generate MjSpec + spec = builder.generate_mjspec(param) + print("✓ MjSpec generated successfully!") + + # Check structure + bodies = [] + def collect_bodies(body): + bodies.append(body.name) + for child in body.body: + collect_bodies(child) + + collect_bodies(spec.worldbody) + + print("\nBody structure:") + for body_name in bodies: + print(f" - {body_name}") + + print("\n✅ All tests passed! Biped structure updated to standard humanoid format:") + print(" waist geo -> hip joint1 -> hip1 -> hip joint2 -> hip2 -> hip joint3 -> thigh -> knee joint -> calf") + +if __name__ == "__main__": + test_biped() \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..37d7e32 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,3 @@ +{ + "github.copilot.chat.codeGeneration.useInstructionFiles": true +} \ No newline at end of file diff --git a/README.md b/README.md index e37489b..13e4d2f 100644 --- a/README.md +++ b/README.md @@ -5,14 +5,14 @@ A minimalist implementation of morphology randomization in Isaac Sim. Can be use ## TODO: - [x] Preliminary implementation. -- [ ] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. +- [x] `QuadrupedBuilder12Dof` and `QuadrupedBuilder16Dof`. - [ ] A minimal RL baseline. ## Instaillation Git clone and `pip install -e .`. -## Example Usage +## Setup See `scripts/`. @@ -21,6 +21,20 @@ However, IsaacSim uses a custom USD build and is **incompatible** with the one i If you experience IsaacSim errors after `pip install usd-core`, uninstall it. +## Generate Robots + +```python +#QuadrupedBuilder12Dof +python scripts/quadruped_scene.py --num_envs 32 + +#QuadrupedBuilder16Dof(quadwheel) +python scripts/quawheel_scene.py + +#BipedBuilder +python scripts/biped_scene.py + +``` + **Important** Remember to set `replicate_physics=False` and `enabled_self_collisions=False`! diff --git a/scripts/biped_scene.py b/scripts/biped_scene.py new file mode 100644 index 0000000..86c30dd --- /dev/null +++ b/scripts/biped_scene.py @@ -0,0 +1,219 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for procedural biped generation.") +parser.add_argument("--num_envs", type=int, default=64, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralBipedCfg, BipedBuilder + + +BIPED_CONFIG = ArticulationCfg( + spawn=ProceduralBipedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + # Torso parameters + torso_link_length_range=(0.08, 0.14), + torso_link_width_range=(0.14, 0.20), + torso_link_height_range=(0.30, 0.6), + pelvis_height_range=(0.04, 0.06), + # Hip spacing and hip segment dimensions + hip_spacing_range=(0.12, 0.16), + hip_pitch_link_length_range=(0.025, 0.045), + hip_pitch_link_radius_range=(0.015, 0.035), + hip_roll_link_length_range=(0.025, 0.045), + hip_roll_link_radius_range=(0.015, 0.035), + hip_pitch_link_initroll_range=(0.0, np.pi / 2), + # Leg lengths - closer to human proportions + leg_length_range=(0.4, 0.6), + # Leg proportions + shin_ratio_range=(0.85, 1.15), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + # Hip joints - G1-style neutral position + ".*_hip_pitch_joint": 0.0, + ".*_hip_roll_joint": 0.0, + ".*_hip_yaw_joint": 0.0, + # Knee - straight for standing + ".*_knee_joint": 0.0, + # Ankle + ".*_ankle_pitch_joint": 0.0, + ".*_ankle_roll_joint": 0.0, + }, + pos=(0, 0, 1.2), # Higher initial position to avoid ground collision + ), + actuators={ + # Hip actuators - G1-style configuration + "hip_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_pitch_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, # G1 default kp=75 + damping=2.0, # G1 default kv=2 + armature=0.01017752004, + friction=0.1, # G1 default frictionloss=0.1 + ), + "hip_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_roll_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + "hip_yaw": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_yaw_joint"], + effort_limit_sim=88.0, # G1 spec: actuatorfrcrange="-88 88" + stiffness=75.0, + damping=2.0, + armature=0.01017752004, + friction=0.1, + ), + # Knee actuators + "knee": ImplicitActuatorCfg( + joint_names_expr=[".*_knee_joint"], + effort_limit_sim=139.0, # G1 spec: actuatorfrcrange="-139 139" + stiffness=75.0, + damping=2.0, + armature=0.025101925, + friction=0.1, + ), + # Ankle actuators - lower gains for stability + "ankle_pitch": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_pitch_joint"], + effort_limit_sim=50.0, # G1 spec: actuatorfrcrange="-50 50" + stiffness=20.0, # G1 ankle kp=20 + damping=2.0, # G1 ankle kv=2 + armature=0.00721945, + friction=0.1, + ), + "ankle_roll": ImplicitActuatorCfg( + joint_names_expr=[".*_ankle_roll_joint"], + effort_limit_sim=50.0, + stiffness=20.0, + damping=2.0, + armature=0.00721945, + friction=0.1, + ), + }, +) + + +class ProceduralBipedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Biped robot + biped = BIPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Biped") + + # Contact sensor for feet + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Biped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([3.0, 0.0, 2.0], [0.0, 0.0, 1.0]) + + scene_cfg = ProceduralBipedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False # Important for morphology randomization! + ) + scene = InteractiveScene(scene_cfg) + + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["biped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print("=" * 60) + print("Procedural Biped Robot Info") + print("=" * 60) + print(f"Number of instances: {articulation.num_instances}") + print(f"Joint names: {articulation.joint_names}") + print(f"Body names: {articulation.body_names}") + print(f"Is homogeneous: {articulation.root_physx_view.is_homogeneous}") + + builder = BipedBuilder.get_instance() + print("\nGenerated parameters for each environment:") + for i, param in enumerate(builder.params[:min(5, len(builder.params))]): # Show first 5 only + leg_len = param.hip_yaw_link_length + param.knee_link_length + print( + f" Env {i}: torso=({param.torso_link_length:.3f}×{param.torso_link_width:.3f}×{param.torso_link_height:.3f}), " + f"hip_spacing={param.hip_spacing:.3f}, leg_length={leg_len:.2f}, " + f"hip_pitch_link=(r={param.hip_pitch_link_radius:.3f}, l={param.hip_pitch_link_length:.3f}), " + f"hip_roll_link=(r={param.hip_roll_link_radius:.3f}, l={param.hip_roll_link_length:.3f})" + ) + if len(builder.params) > 5: + print(f" ... and {len(builder.params) - 5} more environments") + print("=" * 60) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + step_count = 0 + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + # Print contact info periodically + step_count += 1 + if step_count % 100 == 0: + foot_forces = contact_sensor.data.net_forces_w + print(f"Step {step_count}: Contact forces shape = {foot_forces.shape}") + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/scripts/quadruped_scene.py b/scripts/quadruped_scene.py new file mode 100644 index 0000000..aee6846 --- /dev/null +++ b/scripts/quadruped_scene.py @@ -0,0 +1,138 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis.") +parser.add_argument("--num_envs", type=int, default=32, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralQuadrupedCfg, QuadrupedBuilder + + +QUADRUPED_CONFIG = ArticulationCfg( + spawn=ProceduralQuadrupedCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + ".*_hip_joint": 0.0, + "F[L,R]_thigh_joint": np.pi / 4, + "R[L,R]_thigh_joint": np.pi / 4, + ".*_calf_joint": -np.pi / 2, + }, + pos=(0, 0, 1.0), + ), + actuators={ + ".*": ImplicitActuatorCfg( + joint_names_expr=[".*"], + effort_limit_sim=1000.0, + stiffness=200.0, + damping=2.0, + armature=0.01, + friction=0.01, + ), + }, +) + + +class ProceduralQuadrupedSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + """Designs the scene.""" + # jetbot = JETBOT_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Jetbot") + quadruped = QUADRUPED_CONFIG.replace(prim_path="{ENV_REGEX_NS}/Quadruped") + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/Quadruped/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + scene_cfg = ProceduralQuadrupedSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["quadruped"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print(articulation.joint_names) + print(articulation.body_names) + # print(articulation.data.default_mass) + print(articulation.root_physx_view.is_homogeneous) + + builder = QuadrupedBuilder.get_instance() + print(builder.params) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.root_physx_view.set_masses(articulation.data.default_mass.sqrt(), torch.arange(articulation.num_instances)) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Simulate physics + while simulation_app.is_running(): + # perform step + articulation.set_joint_position_target(default_joint_pos) + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + print(contact_sensor.data.net_forces_w) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() \ No newline at end of file diff --git a/scripts/quadwheel_scene.py b/scripts/quadwheel_scene.py new file mode 100644 index 0000000..e8985f9 --- /dev/null +++ b/scripts/quadwheel_scene.py @@ -0,0 +1,164 @@ +import argparse +import numpy as np +import torch + +from isaaclab.app import AppLauncher + +# create argparser +parser = argparse.ArgumentParser(description="Example script for metamorphosis quad-wheel robot.") +parser.add_argument("--num_envs", type=int, default=16, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import isaaclab.sim as sim_utils +from isaaclab.assets import ArticulationCfg, AssetBaseCfg +from isaaclab.actuators import ImplicitActuatorCfg +from isaaclab.sensors import ContactSensorCfg, ContactSensor +from isaaclab.scene import InteractiveScene, InteractiveSceneCfg +from metamorphosis.asset_cfg import ProceduralQuadWheelCfg, QuadWheelBuilder + + +QUADWHEEL_CONFIG = ArticulationCfg( + spawn=ProceduralQuadWheelCfg( + activate_contact_sensors=True, + articulation_props=sim_utils.ArticulationRootPropertiesCfg( + enabled_self_collisions=False, + solver_position_iteration_count=4, + solver_velocity_iteration_count=1, + ), + base_length_range=(0.5, 1.0), + base_width_range=(0.3, 0.4), + base_height_range=(0.15, 0.25), + leg_length_range=(0.4, 0.8), + calf_length_ratio=(0.9, 1.0), + wheel_radius_range=(0.08, 0.15), + wheel_width_range=(0.03, 0.06), + ), + init_state=ArticulationCfg.InitialStateCfg( + joint_pos={ + ".*_hip_joint": 0.0, + "F[L,R]_thigh_joint": np.pi / 4, + "R[L,R]_thigh_joint": np.pi / 4, + ".*_calf_joint": -np.pi / 2, + ".*_wheel_joint": 0.0, + }, + pos=(0, 0, 1.0), + ), + actuators={ + "legs": ImplicitActuatorCfg( + joint_names_expr=[".*_hip_joint", ".*_thigh_joint", ".*_calf_joint"], + effort_limit_sim=1000.0, + stiffness=200.0, + damping=2.0, + armature=0.01, + friction=0.01, + ), + "wheels": ImplicitActuatorCfg( + joint_names_expr=[".*_wheel_joint"], + effort_limit_sim=500.0, + stiffness=0.0, # Wheels use velocity control + damping=10.0, + armature=0.01, + friction=0.1, + ), + }, +) + + +class ProceduralQuadWheelSceneCfg(InteractiveSceneCfg): + ground = AssetBaseCfg(prim_path="/World/defaultGroundPlane", spawn=sim_utils.GroundPlaneCfg()) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", + spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + """Designs the scene.""" + quadwheel = QUADWHEEL_CONFIG.replace(prim_path="{ENV_REGEX_NS}/QuadWheel") + contact_sensor = ContactSensorCfg( + prim_path="{ENV_REGEX_NS}/QuadWheel/.*", + track_air_time=True, + history_length=3, + ) + + +def main(): + """Main function.""" + + # Initialize the simulation context + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.0, 0.0, 2.5], [-0.5, 0.0, 0.5]) + + scene_cfg = ProceduralQuadWheelSceneCfg( + num_envs=args_cli.num_envs, + env_spacing=2.5, + replicate_physics=False + ) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + scene.reset() + + articulation = scene.articulations["quadwheel"] + contact_sensor: ContactSensor = scene.sensors["contact_sensor"] + + print(articulation.joint_names) + print(articulation.body_names) + print(f"Total DOF: {articulation.num_joints}") + print(articulation.root_physx_view.is_homogeneous) + + builder = QuadWheelBuilder.get_instance() + print(builder.params) + + root_state = articulation.data.default_root_state.clone() + root_state[:, :3] += scene.env_origins + default_joint_pos = articulation.data.default_joint_pos.clone() + + articulation.root_physx_view.set_masses(articulation.data.default_mass.sqrt(), torch.arange(articulation.num_instances)) + articulation.write_root_pose_to_sim(root_state[:, :7]) + articulation.write_joint_position_to_sim(default_joint_pos) + + # Now we are ready! + print("[INFO]: Setup complete...") + + # Create wheel velocity command (forward rolling) + wheel_velocity = torch.zeros((args_cli.num_envs, articulation.num_joints), device=sim.device) + # Set wheel joints to rotate forward (indices 12-15 for the 4 wheels) + wheel_velocity[:, 12:16] = 5.0 # 5 rad/s forward rotation + + # Simulate physics + count = 0 + while simulation_app.is_running(): + # perform step + joint_pos_target = default_joint_pos.clone() + + # Simple demo: make wheels spin after 100 steps + if count > 100: + articulation.set_joint_velocity_target(wheel_velocity) + else: + articulation.set_joint_position_target(joint_pos_target) + + scene.write_data_to_sim() + sim.step() + scene.update(sim.get_physics_dt()) + + if count % 100 == 0: + print(f"Step {count}: Contact forces shape: {contact_sensor.data.net_forces_w.shape}") + count += 1 + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/src/metamorphosis/asset_cfg.py b/src/metamorphosis/asset_cfg.py index a11442b..cc237a8 100644 --- a/src/metamorphosis/asset_cfg.py +++ b/src/metamorphosis/asset_cfg.py @@ -4,7 +4,7 @@ from isaaclab.sim.utils import get_current_stage, find_matching_prim_paths from typing import Callable -from metamorphosis.builder import QuadrupedBuilder, QuadrupedParam +from metamorphosis.builder import QuadrupedBuilder, BipedBuilder, QuadWheelBuilder def spawn( @@ -72,8 +72,170 @@ class ProceduralQuadrupedCfg(SpawnerCfg): calf_length_ratio: tuple[float, float] = (0.9, 1.0) """Range for the calf length ratio.""" - parallel_abduction: float = 0.5 - """Probability of using parallel abduction/adduction configuration.""" +def spawn_biped( + prim_path: str, + cfg: "ProceduralBipedCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = BipedBuilder( + torso_link_length_range=cfg.torso_link_length_range, + torso_link_width_range=cfg.torso_link_width_range, + torso_link_height_range=cfg.torso_link_height_range, + pelvis_height_range=cfg.pelvis_height_range, + hip_spacing_range=cfg.hip_spacing_range, + hip_pitch_link_length_range=cfg.hip_pitch_link_length_range, + hip_pitch_link_radius_range=cfg.hip_pitch_link_radius_range, + hip_roll_link_length_range=cfg.hip_roll_link_length_range, + hip_roll_link_radius_range=cfg.hip_roll_link_radius_range, + hip_pitch_link_initroll_range=cfg.hip_pitch_link_initroll_range, + leg_length_range=cfg.leg_length_range, + shin_ratio_range=cfg.shin_ratio_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralBipedCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural biped (legs only).""" + + func: Callable = spawn_biped + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + torso_link_length_range: tuple[float, float] = (0.10, 0.16) + """Range for the torso length (box X dimension) in meters.""" + + torso_link_width_range: tuple[float, float] = (0.18, 0.26) + """Range for the torso width (box Y dimension) in meters.""" + + torso_link_height_range: tuple[float, float] = (0.08, 0.14) + """Range for the torso height (box Z dimension) in meters.""" + + pelvis_height_range: tuple[float, float] = (0.05, 0.08) + """Range for the waist/pelvis cylinder height in meters.""" + + hip_spacing_range: tuple[float, float] = (0.16, 0.24) + """Range for the distance between left and right hips in meters.""" + + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_pitch_link length in meters.""" + + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_pitch_link radius in meters.""" + + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06) + """Range for hip_roll_link length in meters.""" + + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04) + """Range for hip_roll_link radius in meters.""" + + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, 1.5707963267948966) + """Range for hip pitch link initial roll angle in radians (0 to 90 degrees).""" + + leg_length_range: tuple[float, float] = (0.5, 0.7) + """Range for the total leg length in meters.""" + + shin_ratio_range: tuple[float, float] = (0.85, 1.15) + """Range for shin length as a ratio of thigh length.""" + + +def spawn_quadwheel( + prim_path: str, + cfg: "ProceduralQuadWheelCfg", + translation: tuple[float, float, float] | None = None, + orientation: tuple[float, float, float, float] | None = None, +): + stage = get_current_stage() + builder = QuadWheelBuilder( + base_length_range=cfg.base_length_range, + base_width_range=cfg.base_width_range, + base_height_range=cfg.base_height_range, + leg_length_range=cfg.leg_length_range, + calf_length_ratio=cfg.calf_length_ratio, + wheel_radius_range=cfg.wheel_radius_range, + wheel_width_range=cfg.wheel_width_range, + ) + + root_path, asset_path = prim_path.rsplit("/", 1) + source_prim_paths = find_matching_prim_paths(root_path) + prim_paths = [ + f"{source_prim_path}/{asset_path}" for source_prim_path in source_prim_paths + ] + for i, prim_path in enumerate(prim_paths): + param = builder.sample_params(seed=i) + prim = builder.spawn(stage, prim_path, param) + schemas.modify_articulation_root_properties(prim_path, cfg.articulation_props) + if cfg.activate_contact_sensors: + schemas.activate_contact_sensors(prim_path, stage=stage) + return prim + + +@configclass +class ProceduralQuadWheelCfg(SpawnerCfg): + """Configuration parameters for spawning a procedural quad-wheel robot.""" + + func: Callable = spawn_quadwheel + """Function to use for spawning the asset.""" + + activate_contact_sensors: bool = True + """Whether to activate contact sensors for the asset. Defaults to True.""" + + articulation_props: schemas.ArticulationRootPropertiesCfg | None = None + + visible: bool = True + """Whether the spawned asset should be visible.""" + + semantic_tags: list[tuple[str, str]] | None = None + """List of semantic tags to add to the spawned asset.""" + + copy_from_source: bool = False + """Whether to copy the asset from the source prim or inherit it.""" + + base_length_range: tuple[float, float] = (0.5, 1.0) + """Range for the base length.""" + + base_width_range: tuple[float, float] = (0.3, 0.4) + """Range for the base width.""" + + base_height_range: tuple[float, float] = (0.15, 0.25) + """Range for the base height.""" + + leg_length_range: tuple[float, float] = (0.4, 0.8) + """Range for the leg length.""" + + calf_length_ratio: tuple[float, float] = (0.9, 1.0) + """Range for the calf length ratio.""" + + wheel_radius_range: tuple[float, float] = (0.08, 0.15) + """Range for the wheel radius in meters.""" -__all__ = ["ProceduralQuadrupedCfg", "QuadrupedBuilder", "QuadrupedParam"] + wheel_width_range: tuple[float, float] = (0.03, 0.06) + """Range for the wheel width in meters.""" diff --git a/src/metamorphosis/builder/__init__.py b/src/metamorphosis/builder/__init__.py index 22b4999..b289a25 100644 --- a/src/metamorphosis/builder/__init__.py +++ b/src/metamorphosis/builder/__init__.py @@ -1,3 +1,5 @@ -from .quadruped import QuadrupedBuilder, QuadrupedParam +from .quadruped import QuadrupedBuilder +from .biped import BipedBuilder +from .quadwheel import QuadWheelBuilder -__all__ = ["QuadrupedBuilder", "QuadrupedParam"] \ No newline at end of file +__all__ = ["QuadrupedBuilder", "BipedBuilder", "QuadWheelBuilder"] diff --git a/src/metamorphosis/builder/biped.py b/src/metamorphosis/builder/biped.py index e69de29..e86d61e 100644 --- a/src/metamorphosis/builder/biped.py +++ b/src/metamorphosis/builder/biped.py @@ -0,0 +1,436 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class BipedParam(NamedTuple): + """Parameters for a bipedal robot with box pelvis and standard humanoid hip structure.""" + # Torso (main body - box) + torso_link_length: float + torso_link_width: float + torso_link_height: float + torso_link_mass: float + # Waist connection + pelvis_height: float # waist connection height + pelvis_radius: float # waist radius (small cylinder) + + # Hip positioning and dimensions + hip_spacing: float # left-right hip spacing + + # Hip_pitch_link parameters (first hip segment) + hip_pitch_link_length: float # hip pitch link length + hip_pitch_link_radius: float # hip pitch link radius + hip_pitch_link_mass: float # hip pitch link mass + + # Hip_roll_link parameters (second hip segment) + hip_roll_link_length: float # hip roll link length + hip_roll_link_radius: float # hip roll link radius + hip_roll_link_mass: float # hip roll link mass + + # Hip pitch roll initialization + hip_pitch_link_initroll: float # initial outward roll angle of hip pitch link around X-axis + + # hip_yaw_link parameters + hip_yaw_link_length: float # thigh length + hip_yaw_link_radius: float # thigh radius + hip_yaw_link_mass: float # thigh mass + + # knee_link parameters + knee_link_length: float # shin length + knee_link_radius: float # shin radius + knee_link_mass: float # shin mass + + # ankle_pitch_link parameters + ankle_pitch_link_length: float # ankle pitch link length + ankle_pitch_link_radius: float # ankle pitch link radius + ankle_pitch_link_mass: float # ankle pitch link mass + + # ankle_roll_link parameters + ankle_roll_link_length: float # foot length + ankle_roll_link_width: float # foot width + ankle_roll_link_height: float # foot height + ankle_roll_link_mass: float # foot mass + + +class BipedBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized bipedal robots (legs only). + + This class creates bipedal robot models with configurable body dimensions + and leg proportions. It samples random parameters within specified ranges + and generates MuJoCo specifications for the robot model. + + The biped uses a standard humanoid structure: + - A pelvis (base body) with configurable dimensions + - Two legs, each with: + - Waist connection geometry (cylinder) + - Hip joint 1 (pitch) -> Hip1 body (capsule) + - Hip joint 2 (roll) -> Hip2 body (capsule) + - Hip joint 3 (yaw) -> Thigh body (capsule) + - Knee joint (pitch) -> Shin/Calf body (capsule) + - Ankle pitch joint (virtual body) + - Ankle roll joint -> Foot body (box) + + This design follows: pelvis geo -> hip_pitch_joint -> hip_pitch_link -> hip_roll_joint -> hip_roll_link -> + hip_yaw_joint -> hip_yaw_link -> knee joint -> knee_link -> ankle_pitch_joint -> ankle_pitch_link -> ankle_roll_joint -> ankle_roll_link (foot) + + Attributes: + torso_link_length_range/torso_link_width_range/torso_link_height_range: Box torso dimensions. + pelvis_height_range: Range for waist cylinder height (also used for offset below torso). + hip_spacing_range: Left/right hip separation. + hip_pitch_link_length_range/hip_pitch_link_radius_range: First hip segment size ranges. + hip_roll_link_length_range/hip_roll_link_radius_range: Second hip segment size ranges. + hip_pitch_link_initroll_range: Range for initial outward roll of hip pitch bodies. + leg_length_range: Range for total leg length (thigh + shin). + shin_ratio_range: Ratio of shin to thigh length. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = BipedBuilder( + ... pelvis_height_range=(0.10, 0.15), + ... leg_length_range=(0.6, 1.0) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + torso_link_length_range: tuple[float, float] = (0.10, 0.16), + torso_link_width_range: tuple[float, float] = (0.18, 0.26), + torso_link_height_range: tuple[float, float] = (0.5, 2.0), + pelvis_height_range: tuple[float, float] = (0.05, 0.08), + hip_spacing_range: tuple[float, float] = (0.16, 0.24), + hip_pitch_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip1 length range + hip_pitch_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip1 radius range + hip_roll_link_length_range: tuple[float, float] = (0.03, 0.06), # Hip2 length range + hip_roll_link_radius_range: tuple[float, float] = (0.02, 0.04), # Hip2 radius range + hip_pitch_link_initroll_range: tuple[float, float] = (0.0, np.pi / 2), + leg_length_range: tuple[float, float] = (0.5, 0.7), + shin_ratio_range: tuple[float, float] = (0.85, 1.15), + valid_filter: Callable[[BipedParam], bool] = lambda _: True, + ): + super().__init__() + self.torso_link_length_range = torso_link_length_range + self.torso_link_width_range = torso_link_width_range + self.torso_link_height_range = torso_link_height_range + self.pelvis_height_range = pelvis_height_range + self.hip_spacing_range = hip_spacing_range + self.hip_pitch_link_length_range = hip_pitch_link_length_range + self.hip_pitch_link_radius_range = hip_pitch_link_radius_range + self.hip_roll_link_length_range = hip_roll_link_length_range + self.hip_roll_link_radius_range = hip_roll_link_radius_range + self.hip_pitch_link_initroll_range = hip_pitch_link_initroll_range + self.leg_length_range = leg_length_range + self.shin_ratio_range = shin_ratio_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int = -1) -> BipedParam: + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + # Torso (box) + torso_link_length = random.uniform(*self.torso_link_length_range) + torso_link_width = random.uniform(*self.torso_link_width_range) + torso_link_height = random.uniform(*self.torso_link_height_range) + torso_link_mass = random.uniform(3.0, 5.0) + + # Waist connection + pelvis_height = random.uniform(*self.pelvis_height_range) + pelvis_radius = min(torso_link_width, torso_link_length) * random.uniform(0.2, 0.4) + # Hip spacing and hip segment dimensions + hip_spacing = random.uniform(*self.hip_spacing_range) + + # Hip pitch segment (first hip) + hip_pitch_link_length = random.uniform(*self.hip_pitch_link_length_range) + hip_pitch_link_radius = random.uniform(*self.hip_pitch_link_radius_range) + hip_pitch_link_mass = random.uniform(0.4, 0.8) + + # Hip roll segment (second hip) + hip_roll_link_length = random.uniform(*self.hip_roll_link_length_range) + hip_roll_link_radius = random.uniform(*self.hip_roll_link_radius_range) + hip_roll_link_mass = random.uniform(0.4, 0.8) + + # Hip pitch initial roll (0 to 90 degrees) + hip_pitch_link_initroll = random.uniform(*self.hip_pitch_link_initroll_range) + + # Leg dimensions: split total leg into thigh (hip_yaw_link) and shin (knee_link) + total_leg_length = random.uniform(*self.leg_length_range) + shin_ratio = random.uniform(*self.shin_ratio_range) + hip_yaw_link_length = total_leg_length / (1 + shin_ratio) + knee_link_length = hip_yaw_link_length * shin_ratio + + # Leg masses and radii (proportional to length) + hip_yaw_link_radius = random.uniform(0.025, 0.040) + hip_yaw_link_mass = hip_yaw_link_length * random.uniform(1.5, 2.2) + + knee_link_radius = hip_yaw_link_radius * random.uniform(0.75, 0.95) + knee_link_mass = knee_link_length * random.uniform(1.2, 1.8) + + # Foot parameters (ankle roll link) + ankle_roll_link_length = random.uniform(0.18, 0.25) + ankle_roll_link_width = random.uniform(0.06, 0.10) + ankle_roll_link_height = random.uniform(0.02, 0.03) + ankle_roll_link_mass = random.uniform(0.2, 0.5) + + # Ankle pitch link (virtual body) derived from foot size + ankle_pitch_link_radius = min(ankle_roll_link_width, ankle_roll_link_height) / 2.0 + ankle_pitch_link_length = ankle_pitch_link_radius * 2.0 + ankle_pitch_link_mass = 0.3 + + param = BipedParam( + torso_link_length=torso_link_length, + torso_link_width=torso_link_width, + torso_link_height=torso_link_height, + torso_link_mass=torso_link_mass, + pelvis_height=pelvis_height, + pelvis_radius=pelvis_radius, + hip_spacing=hip_spacing, + hip_pitch_link_length=hip_pitch_link_length, + hip_pitch_link_radius=hip_pitch_link_radius, + hip_pitch_link_mass=hip_pitch_link_mass, + hip_roll_link_length=hip_roll_link_length, + hip_roll_link_radius=hip_roll_link_radius, + hip_roll_link_mass=hip_roll_link_mass, + hip_pitch_link_initroll=hip_pitch_link_initroll, + hip_yaw_link_length=hip_yaw_link_length, + hip_yaw_link_radius=hip_yaw_link_radius, + hip_yaw_link_mass=hip_yaw_link_mass, + knee_link_length=knee_link_length, + knee_link_radius=knee_link_radius, + knee_link_mass=knee_link_mass, + ankle_pitch_link_length=ankle_pitch_link_length, + ankle_pitch_link_radius=ankle_pitch_link_radius, + ankle_pitch_link_mass=ankle_pitch_link_mass, + ankle_roll_link_length=ankle_roll_link_length, + ankle_roll_link_width=ankle_roll_link_width, + ankle_roll_link_height=ankle_roll_link_height, + ankle_roll_link_mass=ankle_roll_link_mass, + ) + + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + + return param + + def generate_mjspec(self, param: BipedParam) -> mujoco.MjSpec: + spec = mujoco.MjSpec() + + # ============ Torso ============ + # Add torso body + torso_body = spec.worldbody.add_body(name="torso_link") + torso_body.mass = param.torso_link_mass + # Torso inertia + torso_link_ixx = param.torso_link_mass * (param.torso_link_width**2 + param.torso_link_height**2) / 12 + torso_link_iyy = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_height**2) / 12 + torso_link_izz = param.torso_link_mass * (param.torso_link_length**2 + param.torso_link_width**2) / 12 + torso_body.inertia = [torso_link_ixx, torso_link_iyy, torso_link_izz] + # Torso geometry + torso_geom = torso_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + torso_geom.size = [param.torso_link_length/2, param.torso_link_width/2, param.torso_link_height/2] + torso_geom.rgba = [0.75, 0.75, 0.75, 1.0] + + # ============ Pelvis ============ + # Add pelvis + pelvis = torso_body.add_body(name="pelvis") + pelvis.pos = [0, 0, -(param.torso_link_height/2 + param.pelvis_height/2)] + pelvis.mass = 0.5 + # Pelvis inertia + pelvis.inertia = [0.01, 0.01, 0.01] + # Pelvis geometry + pelvis_link = pelvis.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + pelvis_link.size = [param.pelvis_radius, param.pelvis_height/2, 0] + pelvis_link.rgba = [0.65, 0.65, 0.85, 1.0] + + # ============ LEFT AND RIGHT LEGS (6 DOF each) ============ + for side, y_sign in [("left", 1), ("right", -1)]: + + # --- hip_pitch_joint --- + # Add hip_pitch_joint body + hip_pitch_joint = pelvis.add_body(name=f"{side}_hip_pitch_joint") + hip_pitch_joint.pos = [0, y_sign * param.hip_spacing/2, -(param.pelvis_height/2)] + hip_pitch_roll = param.hip_pitch_link_initroll if side == "left" else -param.hip_pitch_link_initroll + hip_pitch_joint.quat = [ + np.cos(hip_pitch_roll / 2.0), + np.sin(hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_pitch_joint.mass = param.hip_pitch_link_mass + # Add axis and rotation center for hip_pitch_joint + pitch_joint = hip_pitch_joint.add_joint( + name=f"{side}_hip_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [1, 0, 0] # X-axis (roll) + pitch_joint.range = [-2.5307, 2.8798] + pitch_joint.armature = 0.01017752004 + + # --- hip_pitch_link --- + # Add hip_pitch_link inertia + hip_pitch_link_ixx = param.hip_pitch_link_mass * (3 * param.hip_pitch_link_radius**2 + param.hip_pitch_link_length**2) / 12 + hip_pitch_link_iyy = param.hip_pitch_link_mass * param.hip_pitch_link_radius**2 / 2 + hip_pitch_link_izz = hip_pitch_link_ixx + hip_pitch_joint.inertia = [hip_pitch_link_ixx, hip_pitch_link_iyy, hip_pitch_link_izz] + # Add hip_pitch_link geometry + hip_pitch_link = hip_pitch_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_pitch_link.name = f"{side}_hip_pitch_link" + hip_pitch_link.size = [param.hip_pitch_link_radius, 0, 0] + hip_pitch_link.fromto = [0, 0, 0, 0, 0, -param.hip_pitch_link_length] + hip_pitch_link.rgba = [0.85, 0.45, 0.45, 1.0] + + # --- hip_roll_joint --- + # Add hip_roll_joint body + hip_roll_joint = hip_pitch_joint.add_body(name=f"{side}_hip_roll_joint") + hip_roll_joint.pos = [0, 0, -(param.hip_pitch_link_length + param.hip_pitch_link_radius + param.hip_roll_link_radius)] # Tangent to hip_pitch_link + hip_roll_joint.quat = [ + np.cos(-hip_pitch_roll / 2.0), + np.sin(-hip_pitch_roll / 2.0), + 0.0, + 0.0, + ] + hip_roll_joint.mass = param.hip_roll_link_mass + # Add axis and rotation center for hip_roll_joint + roll_joint = hip_roll_joint.add_joint( + name=f"{side}_hip_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + roll_joint.axis = [1, 0, 0] # X-axis (roll) + # Different ranges for left/right legs + if side == "left": + roll_joint.range = [-0.5236, 2.9671] + else: + roll_joint.range = [-2.9671, 0.5236] + roll_joint.armature = 0.025101925 + + # --- hip_roll_link --- + # Add hip_roll_link inertia + hip_roll_link_ixx = param.hip_roll_link_mass * (3 * param.hip_roll_link_radius**2 + param.hip_roll_link_length**2) / 12 + hip_roll_link_iyy = param.hip_roll_link_mass * param.hip_roll_link_radius**2 / 2 + hip_roll_link_izz = hip_roll_link_ixx + hip_roll_joint.inertia = [hip_roll_link_ixx, hip_roll_link_iyy, hip_roll_link_izz] + # Add hip_roll_link geometry + hip_roll_link = hip_roll_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_roll_link.name = f"{side}_hip_roll_link" + hip_roll_link.size = [param.hip_roll_link_radius, 0, 0] + hip_roll_link.fromto = [0, 0, 0, 0, 0, -param.hip_roll_link_length] + hip_roll_link.rgba = [0.45, 0.75, 0.45, 1.0] + + # --- hip_yaw_joint --- + # Add hip_yaw_joint body + hip_yaw_joint = hip_roll_joint.add_body(name=f"{side}_hip_yaw_joint") + hip_yaw_joint.pos = [0, 0, -(param.hip_roll_link_length + param.hip_roll_link_radius + param.hip_yaw_link_radius)] # Tangent to hip_roll_link + hip_yaw_joint.mass = param.hip_yaw_link_mass + hip_yaw_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for hip_yaw_joint + yaw_joint = hip_yaw_joint.add_joint( + name=f"{side}_hip_yaw_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + yaw_joint.axis = [0, 0, 1] # Z-axis (yaw) + yaw_joint.range = [-2.7576, 2.7576] + yaw_joint.armature = 0.01017752004 + + # --- hip_yaw_link --- + # Add hip_yaw_link inertia + hip_yaw_link_ixx = param.hip_yaw_link_mass * (3 * param.hip_yaw_link_radius**2 + param.hip_yaw_link_length**2) / 12 + hip_yaw_link_iyy = param.hip_yaw_link_mass * param.hip_yaw_link_radius**2 / 2 + hip_yaw_link_izz = hip_yaw_link_ixx + hip_yaw_joint.inertia = [hip_yaw_link_ixx, hip_yaw_link_iyy, hip_yaw_link_izz] + # Add hip_yaw_link geometry + hip_yaw_geom = hip_yaw_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + hip_yaw_geom.name = f"{side}_hip_yaw_link" + hip_yaw_geom.size = [param.hip_yaw_link_radius, 0, 0] + hip_yaw_geom.fromto = [0, 0, 0, 0, 0, -param.hip_yaw_link_length] + hip_yaw_geom.rgba = [0.45, 0.45, 0.85, 1.0] + + # --- knee_joint --- + # Add knee_joint body + knee_joint = hip_yaw_joint.add_body(name=f"{side}_knee_joint") + knee_joint.pos = [0, 0, -(param.hip_yaw_link_length + param.hip_yaw_link_radius + param.knee_link_radius)] + knee_joint.mass = param.knee_link_mass + knee_joint.quat = [1.0, 0.0, 0.0, 0.0] + # Add axis and rotation center for knee_joint + pitch_joint = knee_joint.add_joint( + name=f"{side}_knee_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + pitch_joint.range = [-0.087267, 2.8798] + pitch_joint.armature = 0.025101925 + + # --- knee_link --- + # Shin inertia + knee_link_ixx = param.knee_link_mass * (3 * param.knee_link_radius**2 + param.knee_link_length**2) / 12 + knee_link_iyy = param.knee_link_mass * param.knee_link_radius**2 / 2 + knee_link_izz = knee_link_ixx + knee_joint.inertia = [knee_link_ixx, knee_link_iyy, knee_link_izz] + + # Knee geometry - capsule + knee_link_geom = knee_joint.add_geom(type=mujoco.mjtGeom.mjGEOM_CAPSULE) + knee_link_geom.name = f"{side}_knee_link" + knee_link_geom.size = [param.knee_link_radius, 0, 0] + knee_link_geom.fromto = [0, 0, 0, 0, 0, -param.knee_link_length] + knee_link_geom.rgba = [0.85, 0.75, 0.35, 1.0] + + # --- ANKLE JOINT 1: PITCH --- + ankle_pitch_link = knee_joint.add_body(name=f"{side}_ankle_pitch_link") + ankle_pitch_link.pos = [0, 0, -(param.knee_link_length + param.knee_link_radius + param.ankle_pitch_link_radius)] # Tangent to knee_link + ankle_pitch_link.mass = param.ankle_pitch_link_mass + + ankle_pitch_joint = ankle_pitch_link.add_joint( + name=f"{side}_ankle_pitch_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_pitch_joint.axis = [0, 1, 0] # Y-axis (pitch) + ankle_pitch_joint.range = [-0.87267, 0.5236] + ankle_pitch_joint.armature = 0.00721945 + + ankle_pitch_link.inertia = [0.005, 0.005, 0.005] + + ankle_pitch_geom = ankle_pitch_link.add_geom(type=mujoco.mjtGeom.mjGEOM_SPHERE) + ankle_pitch_geom.size = [param.ankle_pitch_link_radius, 0, 0] + ankle_pitch_geom.rgba = [0.55, 0.85, 0.85, 1.0] + + # --- ANKLE JOINT 2: ROLL (FOOT) --- + ankle_roll_link = ankle_pitch_link.add_body(name=f"{side}_ankle_roll_link") + ankle_roll_link.pos = [0, 0, -(param.ankle_pitch_link_radius + param.ankle_roll_link_height/2)] # Tangent to ankle_pitch_link + ankle_roll_link.mass = param.ankle_roll_link_mass + + ankle_roll_joint = ankle_roll_link.add_joint( + name=f"{side}_ankle_roll_joint", + type=mujoco.mjtJoint.mjJNT_HINGE + ) + ankle_roll_joint.axis = [1, 0, 0] # X-axis (roll) + ankle_roll_joint.range = [-0.2618, 0.2618] + ankle_roll_joint.armature = 0.00721945 + + # Foot inertia + foot_ixx = param.ankle_roll_link_mass * (param.ankle_roll_link_width**2 + param.ankle_roll_link_height**2) / 12 + foot_iyy = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_height**2) / 12 + foot_izz = param.ankle_roll_link_mass * (param.ankle_roll_link_length**2 + param.ankle_roll_link_width**2) / 12 + ankle_roll_link.inertia = [foot_ixx, foot_iyy, foot_izz] + + # Foot geometry - box extending forward + foot_geom = ankle_roll_link.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + foot_geom.size = [param.ankle_roll_link_length/2, param.ankle_roll_link_width/2, param.ankle_roll_link_height/2] + foot_geom.pos = [param.ankle_roll_link_length/4, 0, -param.ankle_roll_link_height/2] + foot_geom.rgba = [0.85, 0.55, 0.85, 1.0] + + return spec + + +if __name__ == "__main__": + builder = BipedBuilder() + param = builder.sample_params(seed=0) + print(param) + spec = builder.generate_mjspec(param) + print("Generated biped MjSpec successfully!") diff --git a/src/metamorphosis/builder/quadwheel.py b/src/metamorphosis/builder/quadwheel.py new file mode 100644 index 0000000..18608b2 --- /dev/null +++ b/src/metamorphosis/builder/quadwheel.py @@ -0,0 +1,178 @@ +import mujoco +import numpy as np +import random + +from typing import NamedTuple, Callable +from metamorphosis.builder.base import BuilderBase +from metamorphosis.utils.mjs_utils import add_capsule_geom_ + + +class QuadWheelParam(NamedTuple): + base_length: float + base_width: float + base_height: float + thigh_length: float + calf_length: float + thigh_radius: float + wheel_radius: float + wheel_width: float + + +class QuadWheelBuilder(BuilderBase): + """ + Builder for generating procedurally parameterized four-wheeled robots. + + This class creates four-wheeled robot models with configurable body dimensions, + leg proportions, and wheel parameters. It samples random parameters within + specified ranges and generates MuJoCo specifications for the robot model. + + The quad-wheel robot consists of: + - A base body (trunk) with configurable length, width, and height + - Four legs, each with: + - A hip joint (abduction/adduction) + - A thigh segment with a knee joint + - A calf segment + - A wheel (cylindrical shape with continuous rotation joint) + + Attributes: + base_length_range: Tuple of (min, max) for base body length in meters. + base_width_range: Tuple of (min, max) for base body width in meters. + base_height_range: Tuple of (min, max) for base body height in meters. + leg_length_range: Tuple of (min, max) for leg length as a ratio of base_length. + calf_length_ratio: Tuple of (min, max) for calf length as a ratio of thigh_length. + wheel_radius_range: Tuple of (min, max) for wheel radius in meters. + wheel_width_range: Tuple of (min, max) for wheel width in meters. + valid_filter: Optional callable to filter valid parameter combinations. + + Example: + >>> builder = QuadWheelBuilder( + ... base_length_range=(0.5, 1.0), + ... leg_length_range=(0.4, 0.8) + ... ) + >>> param = builder.sample_params(seed=42) + >>> spec = builder.generate_mjspec(param) + """ + + def __init__( + self, + base_length_range: tuple[float, float] = (0.5, 1.0), + base_width_range: tuple[float, float] = (0.3, 0.4), + base_height_range: tuple[float, float] = (0.15, 0.25), + leg_length_range: tuple[float, float] = (0.4, 0.8), + calf_length_ratio: tuple[float, float] = (0.9, 1.0), + wheel_radius_range: tuple[float, float] = (0.08, 0.15), + wheel_width_range: tuple[float, float] = (0.03, 0.06), + valid_filter: Callable[[QuadWheelParam], bool] = lambda _: True, + ): + super().__init__() + self.base_length_range = base_length_range + self.base_width_range = base_width_range + self.base_height_range = base_height_range + self.leg_length_range = leg_length_range + self.calf_length_ratio = calf_length_ratio + self.wheel_radius_range = wheel_radius_range + self.wheel_width_range = wheel_width_range + self.valid_filter = valid_filter + + def sample_params(self, seed: int=-1): + if seed >= 0: + np.random.seed(seed) + random.seed(seed) + + for _ in range(10): + base_length = random.uniform(*self.base_length_range) + base_width = random.uniform(*self.base_width_range) + base_height = random.uniform(*self.base_height_range) + thigh_length = base_length * random.uniform(*self.leg_length_range) + calf_length = thigh_length * random.uniform(*self.calf_length_ratio) + thigh_radius = random.uniform(0.03, 0.05) + wheel_radius = random.uniform(*self.wheel_radius_range) + wheel_width = random.uniform(*self.wheel_width_range) + param = QuadWheelParam( + base_length=base_length, + base_width=base_width, + base_height=base_height, + thigh_length=thigh_length, + calf_length=calf_length, + thigh_radius=thigh_radius, + wheel_radius=wheel_radius, + wheel_width=wheel_width, + ) + if self.valid_filter(param): + break + else: + raise ValueError("Failed to sample valid parameters") + return param + + def generate_mjspec(self, param: QuadWheelParam) -> mujoco.MjSpec: + thigh_radius = param.thigh_radius + calf_radius = param.thigh_radius * 0.8 + + spec = mujoco.MjSpec() + base_body = spec.worldbody.add_body() + base_body.name = "base" + base_body.mass = 1.0 + base_body.inertia = [1.0, 1.0, 1.0] + trunk_geom = base_body.add_geom(type=mujoco.mjtGeom.mjGEOM_BOX) + trunk_geom.size = [param.base_length/2, param.base_width/2, param.base_height/2] + + for name, (x, y) in [ + ("FL", (1, -1)), + ("FR", (1, 1)), + ("RL", (-1, -1)), + ("RR", (-1, 1)) + ]: + hip_body = base_body.add_body(name=f"{name}_hip") + hip_body.pos = [0.5 * param.base_length * x, 0.5 * param.base_width * y, 0] + hip_body.mass = 1.0 + hip_body.inertia = [1.0, 1.0, 1.0] + joint = hip_body.add_joint(name=f"{name}_hip_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[1, 0, 0]) + joint.range = [-np.pi * 0.6, +np.pi * 0.6] + add_capsule_geom_(hip_body, radius=thigh_radius * 1.8, fromto=[-0.1, 0, 0, 0.1, 0, 0]) + + thigh_body = hip_body.add_body(name=f"{name}_thigh") + thigh_body.pos = [0.1 * x, 0.1 * y, 0] + thigh_body.mass = 1.0 + thigh_body.inertia = [1.0, 1.0, 1.0] + joint = thigh_body.add_joint(name=f"{name}_thigh_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + joint.range = [-np.pi * 0.8, np.pi * 0.8] + add_capsule_geom_(thigh_body, radius=thigh_radius, fromto=[0, 0, 0, 0, 0, -param.thigh_length]) + + calf_body = thigh_body.add_body(name=f"{name}_calf") + calf_body.pos = [0, 0, -param.thigh_length] + calf_body.mass = 1.0 + calf_body.inertia = [1.0, 1.0, 1.0] + joint = calf_body.add_joint(name=f"{name}_calf_joint", type=mujoco.mjtJoint.mjJNT_HINGE, axis=[0, 1, 0]) + joint.range = [-np.pi, 0] + add_capsule_geom_(calf_body, radius=calf_radius, fromto=[0, 0, 0, 0, 0, -param.calf_length]) + + # Wheel body with continuous rotation joint + # Position wheel at the end of calf, offset to the side (Y direction) + wheel_body = calf_body.add_body(name=f"{name}_wheel") + wheel_body.pos = [0, y * (param.wheel_radius + calf_radius), -param.calf_length] # Offset to outer side + wheel_body.mass = 0.5 + wheel_body.inertia = [0.1, 0.1, 0.1] + + # Continuous rotation joint for wheel (rotation around Y-axis, like car wheels) + wheel_joint = wheel_body.add_joint( + name=f"{name}_wheel_joint", + type=mujoco.mjtJoint.mjJNT_HINGE, + axis=[0, 1, 0] # Rotation around Y-axis (forward/backward rolling) + ) + # No range specified = unlimited rotation + + # Cylinder geometry for wheel (horizontal, axis along Y) + wheel_geom = wheel_body.add_geom(type=mujoco.mjtGeom.mjGEOM_CYLINDER) + wheel_geom.size = [param.wheel_radius, param.wheel_width / 2, 0.] + # Rotate cylinder 90 degrees around X-axis to align with Y-axis (horizontal) + wheel_geom.quat = [0.7071068, 0.7071068, 0, 0] # 90 degree rotation around X-axis + + return spec + + +if __name__ == "__main__": + builder = QuadWheelBuilder() + builder_instance = QuadWheelBuilder.get_instance() + assert builder_instance is builder + param = builder.sample_params() + print(param) diff --git a/src/metamorphosis/utils/usd_utils.py b/src/metamorphosis/utils/usd_utils.py index 106d44f..36daafb 100644 --- a/src/metamorphosis/utils/usd_utils.py +++ b/src/metamorphosis/utils/usd_utils.py @@ -40,6 +40,29 @@ def create_capsule(stage: Usd.Stage, path: str, radius: float, fromto: np.ndarra return capsule +def create_cylinder(stage: Usd.Stage, path: str, radius: float, height: float, quat: np.ndarray = None): + """Create a cylinder geometry in USD. + + Args: + stage: USD stage + path: Path for the cylinder prim + radius: Cylinder radius + height: Cylinder height + quat: Quaternion for orientation [w, x, y, z], if None uses identity + """ + cylinder = UsdGeom.Cylinder.Define(stage, path) + add_default_transform_(cylinder.GetPrim()) + cylinder.CreateAxisAttr("Z") # Default axis + cylinder.CreateRadiusAttr(radius) + cylinder.CreateHeightAttr(height) + + # Apply orientation if provided + if quat is not None: + cylinder.GetPrim().GetAttribute("xformOp:orient").Set(Gf.Quatf(*quat)) + + return cylinder + + def create_fixed_joint(stage: Usd.Stage, path: str, body_0: Usd.Prim, body_1: Usd.Prim): joint = UsdPhysics.FixedJoint.Define(stage, path) joint.CreateBody0Rel().SetTargets([body_0.GetPath()]) @@ -112,6 +135,10 @@ def from_mjspec(stage: Usd.Stage, prim_path: str, spec: mujoco.MjSpec) -> Usd.Pr sphere = UsdGeom.Sphere.Define(stage, geom_path) add_default_transform_(sphere.GetPrim()) sphere.CreateRadiusAttr(geom.size[0]) + case mujoco.mjtGeom.mjGEOM_CYLINDER: + # Extract quaternion if provided, otherwise use None + quat = np.array(geom.quat) if hasattr(geom, 'quat') and geom.quat is not None else None + cylinder = create_cylinder(stage, geom_path, geom.size[0], geom.size[1] * 2, quat) case _: raise ValueError(f"Unsupported geometry type: {geom.type}") add_default_transform_(xform_prim)